CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v20_T_Main

せとうちオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/LineProcess.cpp@0:b910276f9da2, 2016-05-01 (annotated)

Committer:
lilac0112_1
Date:
Sun May 01 06:30:37 2016 +0000
Revision:
0:b910276f9da2
from 2v10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:b910276f9da2 1 #include "mbed.h"
lilac0112_1 0:b910276f9da2 2 #include "extern.h"
lilac0112_1 0:b910276f9da2 3
lilac0112_1 0:b910276f9da2 4 void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 5 uint8_t LineState;
lilac0112_1 0:b910276f9da2 6 uint8_t LineSum;
lilac0112_1 0:b910276f9da2 7
lilac0112_1 0:b910276f9da2 8 //line
lilac0112_1 0:b910276f9da2 9 LineState = 0;
lilac0112_1 0:b910276f9da2 10
lilac0112_1 0:b910276f9da2 11 LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1);
lilac0112_1 0:b910276f9da2 12 if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){
lilac0112_1 0:b910276f9da2 13 LineState = 3;
lilac0112_1 0:b910276f9da2 14 }
lilac0112_1 0:b910276f9da2 15 else if(LineSum==2){
lilac0112_1 0:b910276f9da2 16 LineState = 2;
lilac0112_1 0:b910276f9da2 17 }
lilac0112_1 0:b910276f9da2 18 else if(LineSum==1){
lilac0112_1 0:b910276f9da2 19 LineState = 1;
lilac0112_1 0:b910276f9da2 20 }
lilac0112_1 0:b910276f9da2 21 else if(LineSum==0){
lilac0112_1 0:b910276f9da2 22 LineState = 0;
lilac0112_1 0:b910276f9da2 23 }
lilac0112_1 0:b910276f9da2 24
lilac0112_1 0:b910276f9da2 25
lilac0112_1 0:b910276f9da2 26 if(pow_x>=0){
lilac0112_1 0:b910276f9da2 27 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 0:b910276f9da2 28 else *x=1;
lilac0112_1 0:b910276f9da2 29 }
lilac0112_1 0:b910276f9da2 30 else{
lilac0112_1 0:b910276f9da2 31 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 0:b910276f9da2 32 else *x=1;
lilac0112_1 0:b910276f9da2 33 }
lilac0112_1 0:b910276f9da2 34
lilac0112_1 0:b910276f9da2 35 if(pow_y>=0){
lilac0112_1 0:b910276f9da2 36 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 0:b910276f9da2 37 else *y=1;
lilac0112_1 0:b910276f9da2 38 }
lilac0112_1 0:b910276f9da2 39 else{
lilac0112_1 0:b910276f9da2 40 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 0:b910276f9da2 41 else *y=1;
lilac0112_1 0:b910276f9da2 42 }
lilac0112_1 0:b910276f9da2 43 }
lilac0112_1 0:b910276f9da2 44 void LineJudgeSlow2(double pow_x, double pow_y, double *x, double *y, uint8_t *px, uint8_t *py){
lilac0112_1 0:b910276f9da2 45 uint8_t LineState;
lilac0112_1 0:b910276f9da2 46 uint8_t LineSum;
lilac0112_1 0:b910276f9da2 47
lilac0112_1 0:b910276f9da2 48 //line
lilac0112_1 0:b910276f9da2 49 LineState = 0;
lilac0112_1 0:b910276f9da2 50
lilac0112_1 0:b910276f9da2 51 LineSum = (data.lnRawOrder[0]!=LINE_EMPTY)+(data.lnRawOrder[1]!=LINE_EMPTY)+(data.lnRawOrder[2]!=LINE_EMPTY);
lilac0112_1 0:b910276f9da2 52 if(LineSum==3){
lilac0112_1 0:b910276f9da2 53 LineState = 3;
lilac0112_1 0:b910276f9da2 54 }
lilac0112_1 0:b910276f9da2 55 else if(LineSum==2){
lilac0112_1 0:b910276f9da2 56 LineState = 2;
lilac0112_1 0:b910276f9da2 57 }
lilac0112_1 0:b910276f9da2 58 else if(LineSum==1){
lilac0112_1 0:b910276f9da2 59 LineState = 1;
lilac0112_1 0:b910276f9da2 60 }
lilac0112_1 0:b910276f9da2 61 else if(LineSum==0){
lilac0112_1 0:b910276f9da2 62 LineState = 0;
lilac0112_1 0:b910276f9da2 63 }
lilac0112_1 0:b910276f9da2 64
lilac0112_1 0:b910276f9da2 65 if(LineSum>0){
lilac0112_1 0:b910276f9da2 66 if(pow_x>=0){
lilac0112_1 0:b910276f9da2 67 if((data.ping[R_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 68 }
lilac0112_1 0:b910276f9da2 69 else{
lilac0112_1 0:b910276f9da2 70 if((data.ping[L_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 71 }
lilac0112_1 0:b910276f9da2 72
lilac0112_1 0:b910276f9da2 73 if(pow_y>=0){
lilac0112_1 0:b910276f9da2 74 if((data.ping[F_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 75 }
lilac0112_1 0:b910276f9da2 76 else{
lilac0112_1 0:b910276f9da2 77 if((data.ping[B_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 78 }
lilac0112_1 0:b910276f9da2 79 }
lilac0112_1 0:b910276f9da2 80 else if(LineSum==0){
lilac0112_1 0:b910276f9da2 81 ////超音波による減速
lilac0112_1 0:b910276f9da2 82 if(pow_x>=0){
lilac0112_1 0:b910276f9da2 83 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 84 }
lilac0112_1 0:b910276f9da2 85 else{
lilac0112_1 0:b910276f9da2 86 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 87 }
lilac0112_1 0:b910276f9da2 88
lilac0112_1 0:b910276f9da2 89 if(pow_y>=0){
lilac0112_1 0:b910276f9da2 90 //if((data.ping[F_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 91 }
lilac0112_1 0:b910276f9da2 92 else{
lilac0112_1 0:b910276f9da2 93 if((data.ping[B_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 0:b910276f9da2 94 }
lilac0112_1 0:b910276f9da2 95 }
lilac0112_1 0:b910276f9da2 96
lilac0112_1 0:b910276f9da2 97 if((data.lnStayNow[X_LINE]==1)&&((*px)>15)){
lilac0112_1 0:b910276f9da2 98 (*px)=15;
lilac0112_1 0:b910276f9da2 99 }
lilac0112_1 0:b910276f9da2 100 if((data.lnStayNow[Y_LINE]==1)&&((*py)>15)){
lilac0112_1 0:b910276f9da2 101 (*py)=15;
lilac0112_1 0:b910276f9da2 102 }
lilac0112_1 0:b910276f9da2 103
lilac0112_1 0:b910276f9da2 104 ////超音波による減速
lilac0112_1 0:b910276f9da2 105 if(pow_x>=0){
lilac0112_1 0:b910276f9da2 106 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
lilac0112_1 0:b910276f9da2 107 (*px)=20;
lilac0112_1 0:b910276f9da2 108 if((*py)>30) (*py)=30;
lilac0112_1 0:b910276f9da2 109 }
lilac0112_1 0:b910276f9da2 110 }
lilac0112_1 0:b910276f9da2 111 else{
lilac0112_1 0:b910276f9da2 112 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
lilac0112_1 0:b910276f9da2 113 (*px)=20;
lilac0112_1 0:b910276f9da2 114 if((*py)>30) (*py)=30;
lilac0112_1 0:b910276f9da2 115 }
lilac0112_1 0:b910276f9da2 116 }
lilac0112_1 0:b910276f9da2 117
lilac0112_1 0:b910276f9da2 118 if(pow_y>=0){
lilac0112_1 0:b910276f9da2 119 if((data.ping[F_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
lilac0112_1 0:b910276f9da2 120 }
lilac0112_1 0:b910276f9da2 121 else{
lilac0112_1 0:b910276f9da2 122 if((data.ping[B_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
lilac0112_1 0:b910276f9da2 123 }
lilac0112_1 0:b910276f9da2 124
lilac0112_1 0:b910276f9da2 125 *x=1;
lilac0112_1 0:b910276f9da2 126 *y=1;
lilac0112_1 0:b910276f9da2 127 }
lilac0112_1 0:b910276f9da2 128 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 129 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
lilac0112_1 0:b910276f9da2 130 uint8_t LinePingState[4];
lilac0112_1 0:b910276f9da2 131 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 0:b910276f9da2 132 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 0:b910276f9da2 133 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 0:b910276f9da2 134 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 0:b910276f9da2 135 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 0:b910276f9da2 136
lilac0112_1 0:b910276f9da2 137 if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 0:b910276f9da2 138
lilac0112_1 0:b910276f9da2 139 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
lilac0112_1 0:b910276f9da2 140 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
lilac0112_1 0:b910276f9da2 141 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 142 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 143
lilac0112_1 0:b910276f9da2 144 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 0:b910276f9da2 145 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 146 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 147
lilac0112_1 0:b910276f9da2 148 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 149 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 150
lilac0112_1 0:b910276f9da2 151 //x
lilac0112_1 0:b910276f9da2 152 if(data.ReturnDir[X_LINE]==L_LINE){
lilac0112_1 0:b910276f9da2 153 if(pow_x<0){
lilac0112_1 0:b910276f9da2 154 *x = 0;
lilac0112_1 0:b910276f9da2 155 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 156 }
lilac0112_1 0:b910276f9da2 157 else{
lilac0112_1 0:b910276f9da2 158 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 159 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 160 }
lilac0112_1 0:b910276f9da2 161 }
lilac0112_1 0:b910276f9da2 162 else if(data.ReturnDir[X_LINE]==R_LINE){
lilac0112_1 0:b910276f9da2 163 if(pow_x>0){
lilac0112_1 0:b910276f9da2 164 *x = 0;
lilac0112_1 0:b910276f9da2 165 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 166 }
lilac0112_1 0:b910276f9da2 167 else{
lilac0112_1 0:b910276f9da2 168 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 169 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 170 }
lilac0112_1 0:b910276f9da2 171 }
lilac0112_1 0:b910276f9da2 172 else if(data.ReturnDir[X_LINE]==LINE_EMPTY){
lilac0112_1 0:b910276f9da2 173 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
lilac0112_1 0:b910276f9da2 174 /*if(pow_x>0){
lilac0112_1 0:b910276f9da2 175 *x = -pow_x*(1-0.75);
lilac0112_1 0:b910276f9da2 176 }
lilac0112_1 0:b910276f9da2 177 else{
lilac0112_1 0:b910276f9da2 178 *x = pow_x*(1-0.75);
lilac0112_1 0:b910276f9da2 179 }*/
lilac0112_1 0:b910276f9da2 180 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 181 }
lilac0112_1 0:b910276f9da2 182 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 0:b910276f9da2 183
lilac0112_1 0:b910276f9da2 184 data.ReturnDir[X_LINE]=L_LINE;
lilac0112_1 0:b910276f9da2 185
lilac0112_1 0:b910276f9da2 186 /*if(pow_x<0){
lilac0112_1 0:b910276f9da2 187 *x = 0;
lilac0112_1 0:b910276f9da2 188 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 189 }
lilac0112_1 0:b910276f9da2 190 else{
lilac0112_1 0:b910276f9da2 191 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 192 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 193 }*/
lilac0112_1 0:b910276f9da2 194 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 195 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 196 }
lilac0112_1 0:b910276f9da2 197 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 0:b910276f9da2 198
lilac0112_1 0:b910276f9da2 199 data.ReturnDir[X_LINE]=R_LINE;
lilac0112_1 0:b910276f9da2 200
lilac0112_1 0:b910276f9da2 201 /*if(pow_x>0){
lilac0112_1 0:b910276f9da2 202 *x = 0;
lilac0112_1 0:b910276f9da2 203 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 204 }
lilac0112_1 0:b910276f9da2 205 else{
lilac0112_1 0:b910276f9da2 206 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 207 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 208 }*/
lilac0112_1 0:b910276f9da2 209 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 210 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 211 }
lilac0112_1 0:b910276f9da2 212 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 0:b910276f9da2 213
lilac0112_1 0:b910276f9da2 214 if(
lilac0112_1 0:b910276f9da2 215 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==B_SPOT)
lilac0112_1 0:b910276f9da2 216 ){
lilac0112_1 0:b910276f9da2 217 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 218 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 219 }
lilac0112_1 0:b910276f9da2 220 else if(
lilac0112_1 0:b910276f9da2 221 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==A_SPOT)
lilac0112_1 0:b910276f9da2 222 ){
lilac0112_1 0:b910276f9da2 223 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 224 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 225 }
lilac0112_1 0:b910276f9da2 226 else{
lilac0112_1 0:b910276f9da2 227 *x = 0;
lilac0112_1 0:b910276f9da2 228 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 229 }
lilac0112_1 0:b910276f9da2 230 }
lilac0112_1 0:b910276f9da2 231 }
lilac0112_1 0:b910276f9da2 232 //y
lilac0112_1 0:b910276f9da2 233 if(data.ReturnDir[Y_LINE]==F_LINE){
lilac0112_1 0:b910276f9da2 234 if(pow_y>0){
lilac0112_1 0:b910276f9da2 235 *y = 0;
lilac0112_1 0:b910276f9da2 236 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 237 }
lilac0112_1 0:b910276f9da2 238 else{
lilac0112_1 0:b910276f9da2 239 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 240 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 241 }
lilac0112_1 0:b910276f9da2 242 }
lilac0112_1 0:b910276f9da2 243 else if(data.ReturnDir[Y_LINE]==B_LINE){
lilac0112_1 0:b910276f9da2 244 if(pow_y<0){
lilac0112_1 0:b910276f9da2 245 *y = 0;
lilac0112_1 0:b910276f9da2 246 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 247 }
lilac0112_1 0:b910276f9da2 248 else{
lilac0112_1 0:b910276f9da2 249 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 250 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 251 }
lilac0112_1 0:b910276f9da2 252 }
lilac0112_1 0:b910276f9da2 253 else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){
lilac0112_1 0:b910276f9da2 254 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
lilac0112_1 0:b910276f9da2 255 /*if(pow_y>0){
lilac0112_1 0:b910276f9da2 256 *y = -pow_y*(1-0.75);
lilac0112_1 0:b910276f9da2 257 }
lilac0112_1 0:b910276f9da2 258 else{
lilac0112_1 0:b910276f9da2 259 *y = pow_y*(1-0.75);
lilac0112_1 0:b910276f9da2 260 }*/
lilac0112_1 0:b910276f9da2 261 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 262 }
lilac0112_1 0:b910276f9da2 263 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 0:b910276f9da2 264
lilac0112_1 0:b910276f9da2 265 data.ReturnDir[Y_LINE]=B_LINE;
lilac0112_1 0:b910276f9da2 266
lilac0112_1 0:b910276f9da2 267 if(pow_y<0){
lilac0112_1 0:b910276f9da2 268 *y = 0;
lilac0112_1 0:b910276f9da2 269 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 270 }
lilac0112_1 0:b910276f9da2 271 else{
lilac0112_1 0:b910276f9da2 272 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 273 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 274 }
lilac0112_1 0:b910276f9da2 275 }
lilac0112_1 0:b910276f9da2 276 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 0:b910276f9da2 277
lilac0112_1 0:b910276f9da2 278 data.ReturnDir[Y_LINE]=F_LINE;
lilac0112_1 0:b910276f9da2 279
lilac0112_1 0:b910276f9da2 280 if(pow_y>0){
lilac0112_1 0:b910276f9da2 281 *y = 0;
lilac0112_1 0:b910276f9da2 282 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 283 }
lilac0112_1 0:b910276f9da2 284 else{
lilac0112_1 0:b910276f9da2 285 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 286 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 287 }
lilac0112_1 0:b910276f9da2 288 }
lilac0112_1 0:b910276f9da2 289 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 0:b910276f9da2 290 if(
lilac0112_1 0:b910276f9da2 291 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==B_SPOT)||
lilac0112_1 0:b910276f9da2 292 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==A_SPOT)
lilac0112_1 0:b910276f9da2 293 ){
lilac0112_1 0:b910276f9da2 294 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 295 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 296 }
lilac0112_1 0:b910276f9da2 297 else if(
lilac0112_1 0:b910276f9da2 298 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==C_SPOT)||
lilac0112_1 0:b910276f9da2 299 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==C_SPOT)
lilac0112_1 0:b910276f9da2 300 ){
lilac0112_1 0:b910276f9da2 301 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 302 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 303 }
lilac0112_1 0:b910276f9da2 304 else{
lilac0112_1 0:b910276f9da2 305 *y = 0;
lilac0112_1 0:b910276f9da2 306 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 307 }
lilac0112_1 0:b910276f9da2 308 }
lilac0112_1 0:b910276f9da2 309 }
lilac0112_1 0:b910276f9da2 310 //none
lilac0112_1 0:b910276f9da2 311 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){
lilac0112_1 0:b910276f9da2 312 //turn!
lilac0112_1 0:b910276f9da2 313 }
lilac0112_1 0:b910276f9da2 314
lilac0112_1 0:b910276f9da2 315 }
lilac0112_1 0:b910276f9da2 316 else{//data.FieldSpot==LINE_INSIDE
lilac0112_1 0:b910276f9da2 317
lilac0112_1 0:b910276f9da2 318 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 319 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 320
lilac0112_1 0:b910276f9da2 321 *x = 0;
lilac0112_1 0:b910276f9da2 322 *y = 0;
lilac0112_1 0:b910276f9da2 323
lilac0112_1 0:b910276f9da2 324 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 0:b910276f9da2 325 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 326 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 327 /*
lilac0112_1 0:b910276f9da2 328 if(
lilac0112_1 0:b910276f9da2 329 (data.lnRawMemory[A_SPOT]==1)&&
lilac0112_1 0:b910276f9da2 330 (data.lnRawMemory[B_SPOT]==1)&&
lilac0112_1 0:b910276f9da2 331 (data.lnRawMemory[C_SPOT]==0)
lilac0112_1 0:b910276f9da2 332 ){
lilac0112_1 0:b910276f9da2 333 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 334 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 335 }
lilac0112_1 0:b910276f9da2 336 else if(
lilac0112_1 0:b910276f9da2 337 (data.lnRawMemory[A_SPOT]==0)&&
lilac0112_1 0:b910276f9da2 338 (data.lnRawMemory[B_SPOT]==0)&&
lilac0112_1 0:b910276f9da2 339 (data.lnRawMemory[C_SPOT]==1)
lilac0112_1 0:b910276f9da2 340 ){
lilac0112_1 0:b910276f9da2 341 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 342 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 343 }*/
lilac0112_1 0:b910276f9da2 344 }
lilac0112_1 0:b910276f9da2 345 }
lilac0112_1 0:b910276f9da2 346 void LineJudgeReturn2(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 347 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
lilac0112_1 0:b910276f9da2 348 uint8_t LinePingState[4];
lilac0112_1 0:b910276f9da2 349 //JSO2
lilac0112_1 0:b910276f9da2 350 static uint8_t lnRawReturn1 = 0;
lilac0112_1 0:b910276f9da2 351 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 0:b910276f9da2 352 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 0:b910276f9da2 353 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 0:b910276f9da2 354 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 0:b910276f9da2 355 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 0:b910276f9da2 356
lilac0112_1 0:b910276f9da2 357 data.FieldSpot=LINE_INSIDE;
lilac0112_1 0:b910276f9da2 358
lilac0112_1 0:b910276f9da2 359 if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 0:b910276f9da2 360 }
lilac0112_1 0:b910276f9da2 361 else{//data.FieldSpot==LINE_INSIDE
lilac0112_1 0:b910276f9da2 362
lilac0112_1 0:b910276f9da2 363 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 364 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 365
lilac0112_1 0:b910276f9da2 366 *x = 0;
lilac0112_1 0:b910276f9da2 367 *y = 0;
lilac0112_1 0:b910276f9da2 368
lilac0112_1 0:b910276f9da2 369 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 0:b910276f9da2 370 data.lnStop[X_LINE]=1;
lilac0112_1 0:b910276f9da2 371 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 372
lilac0112_1 0:b910276f9da2 373 if(//y
lilac0112_1 0:b910276f9da2 374 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==B_SPOT)&&(1))||
lilac0112_1 0:b910276f9da2 375 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==A_SPOT)&&(1))
lilac0112_1 0:b910276f9da2 376 ){
lilac0112_1 0:b910276f9da2 377 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 378 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 379 //JSO1
lilac0112_1 0:b910276f9da2 380 if(
lilac0112_1 0:b910276f9da2 381 (data.ping[F_PING]<45)&&
lilac0112_1 0:b910276f9da2 382 (data.lnRawReturn==1)//JSO2
lilac0112_1 0:b910276f9da2 383 ){
lilac0112_1 0:b910276f9da2 384 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 385 }
lilac0112_1 0:b910276f9da2 386 //JSO2
lilac0112_1 0:b910276f9da2 387 if(
lilac0112_1 0:b910276f9da2 388 (
lilac0112_1 0:b910276f9da2 389 ((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==0))||
lilac0112_1 0:b910276f9da2 390 ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0))
lilac0112_1 0:b910276f9da2 391 )&&
lilac0112_1 0:b910276f9da2 392 (data.ping[F_PING]>45)&&
lilac0112_1 0:b910276f9da2 393 (data.ping[F_PING]<200)&&
lilac0112_1 0:b910276f9da2 394 (data.ping[R_PING]<25)&&
lilac0112_1 0:b910276f9da2 395 (data.ping[L_PING]>40)
lilac0112_1 0:b910276f9da2 396 ){
lilac0112_1 0:b910276f9da2 397 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 398 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 399 }
lilac0112_1 0:b910276f9da2 400 if(
lilac0112_1 0:b910276f9da2 401 (
lilac0112_1 0:b910276f9da2 402 ((Line[A_SPOT].read()==0)&&(Line[B_SPOT].read()==1))||
lilac0112_1 0:b910276f9da2 403 ((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1))
lilac0112_1 0:b910276f9da2 404 )&&
lilac0112_1 0:b910276f9da2 405 (data.ping[F_PING]>45)&&
lilac0112_1 0:b910276f9da2 406 (data.ping[F_PING]<200)&&
lilac0112_1 0:b910276f9da2 407 (data.ping[L_PING]<25)&&
lilac0112_1 0:b910276f9da2 408 (data.ping[R_PING]>40)
lilac0112_1 0:b910276f9da2 409 ){
lilac0112_1 0:b910276f9da2 410 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 411 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 412 }
lilac0112_1 0:b910276f9da2 413 }
lilac0112_1 0:b910276f9da2 414 else if(
lilac0112_1 0:b910276f9da2 415 ((data.lnRawOrder[0]==C_SPOT)&&(1)&&(1))
lilac0112_1 0:b910276f9da2 416 ){
lilac0112_1 0:b910276f9da2 417 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 418 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 419
lilac0112_1 0:b910276f9da2 420 //JSO1
lilac0112_1 0:b910276f9da2 421 if(
lilac0112_1 0:b910276f9da2 422 (data.ping[B_PING]<45)&&
lilac0112_1 0:b910276f9da2 423 (data.lnRawReturn==1)
lilac0112_1 0:b910276f9da2 424 ){
lilac0112_1 0:b910276f9da2 425 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 426 }
lilac0112_1 0:b910276f9da2 427 }
lilac0112_1 0:b910276f9da2 428 else if(//x
lilac0112_1 0:b910276f9da2 429 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
lilac0112_1 0:b910276f9da2 430 ){
lilac0112_1 0:b910276f9da2 431 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 432 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 433 }
lilac0112_1 0:b910276f9da2 434 else if(
lilac0112_1 0:b910276f9da2 435 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
lilac0112_1 0:b910276f9da2 436 ){
lilac0112_1 0:b910276f9da2 437 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 438 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 439 }
lilac0112_1 0:b910276f9da2 440
lilac0112_1 0:b910276f9da2 441 if(
lilac0112_1 0:b910276f9da2 442 ((*x)!=0)||
lilac0112_1 0:b910276f9da2 443 ((*y)!=0)
lilac0112_1 0:b910276f9da2 444 ){
lilac0112_1 0:b910276f9da2 445
lilac0112_1 0:b910276f9da2 446 data.lnRawReturn=1;
lilac0112_1 0:b910276f9da2 447
lilac0112_1 0:b910276f9da2 448 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWallPlus[X_PING]);
lilac0112_1 0:b910276f9da2 449 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWallPlus[X_PING]);
lilac0112_1 0:b910276f9da2 450 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWallPlus[Y_PING]);
lilac0112_1 0:b910276f9da2 451 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWallPlus[Y_PING]);
lilac0112_1 0:b910276f9da2 452 //x
lilac0112_1 0:b910276f9da2 453 if(data.lnStop[X_LINE]==1){
lilac0112_1 0:b910276f9da2 454 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 0:b910276f9da2 455 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 456 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 457 }
lilac0112_1 0:b910276f9da2 458 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 0:b910276f9da2 459 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 460 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 461 }
lilac0112_1 0:b910276f9da2 462 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 0:b910276f9da2 463 *x = 0;
lilac0112_1 0:b910276f9da2 464 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 465 }
lilac0112_1 0:b910276f9da2 466 }
lilac0112_1 0:b910276f9da2 467 //y
lilac0112_1 0:b910276f9da2 468 if(data.lnStop[Y_LINE]==1){
lilac0112_1 0:b910276f9da2 469 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 0:b910276f9da2 470 if(pow_y<0){
lilac0112_1 0:b910276f9da2 471 *y = 0;
lilac0112_1 0:b910276f9da2 472 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 473 }
lilac0112_1 0:b910276f9da2 474 else{
lilac0112_1 0:b910276f9da2 475 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 476 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 477 }
lilac0112_1 0:b910276f9da2 478 }
lilac0112_1 0:b910276f9da2 479 //JSO2
lilac0112_1 0:b910276f9da2 480 if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)&&(data.lnRawReturn==1)){
lilac0112_1 0:b910276f9da2 481 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 482 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 483 }
lilac0112_1 0:b910276f9da2 484
lilac0112_1 0:b910276f9da2 485 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 0:b910276f9da2 486 if(pow_y>0){
lilac0112_1 0:b910276f9da2 487 *y = 0;
lilac0112_1 0:b910276f9da2 488 data.lnStop[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 489 }
lilac0112_1 0:b910276f9da2 490 else{
lilac0112_1 0:b910276f9da2 491 *y = LineReturn[4];
lilac0112_1 0:b910276f9da2 492 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 493 }
lilac0112_1 0:b910276f9da2 494 }
lilac0112_1 0:b910276f9da2 495 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 0:b910276f9da2 496 *y = 0;
lilac0112_1 0:b910276f9da2 497 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 498 }
lilac0112_1 0:b910276f9da2 499 }
lilac0112_1 0:b910276f9da2 500 }
lilac0112_1 0:b910276f9da2 501 else{
lilac0112_1 0:b910276f9da2 502 data.lnRawReturn=0;
lilac0112_1 0:b910276f9da2 503
lilac0112_1 0:b910276f9da2 504 //JSO3
lilac0112_1 0:b910276f9da2 505 if((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x>0)){
lilac0112_1 0:b910276f9da2 506 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 507 }
lilac0112_1 0:b910276f9da2 508 if((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x<0)){
lilac0112_1 0:b910276f9da2 509 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 510 }
lilac0112_1 0:b910276f9da2 511 //JSO3
lilac0112_1 0:b910276f9da2 512 if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){
lilac0112_1 0:b910276f9da2 513 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 514 }
lilac0112_1 0:b910276f9da2 515 }
lilac0112_1 0:b910276f9da2 516 //JSO3
lilac0112_1 0:b910276f9da2 517 if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){
lilac0112_1 0:b910276f9da2 518 data.lnStop[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 519 }
lilac0112_1 0:b910276f9da2 520 //JSO1
lilac0112_1 0:b910276f9da2 521 if(data.lnRawReturn==1){
lilac0112_1 0:b910276f9da2 522 //JSO2
lilac0112_1 0:b910276f9da2 523 if((lnRawReturn1==0)){
lilac0112_1 0:b910276f9da2 524 LineLiberate();
lilac0112_1 0:b910276f9da2 525 lnRawReturn1=1;
lilac0112_1 0:b910276f9da2 526 }
lilac0112_1 0:b910276f9da2 527
lilac0112_1 0:b910276f9da2 528 if(
lilac0112_1 0:b910276f9da2 529 (
lilac0112_1 0:b910276f9da2 530 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))||
lilac0112_1 0:b910276f9da2 531 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
lilac0112_1 0:b910276f9da2 532 )&&
lilac0112_1 0:b910276f9da2 533 //((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==1))&&//JSO1
lilac0112_1 0:b910276f9da2 534 ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1))&&//JSO2
lilac0112_1 0:b910276f9da2 535 ((data.ping[F_PING]<45)&&(1))&&//JSO2
lilac0112_1 0:b910276f9da2 536 //(!((data.ping[B_PING]<40)&&(data.ping[B_PING]>25)))&&//JSO2
lilac0112_1 0:b910276f9da2 537 ((data.lnRawOrder[0]!=C_SPOT)&&(data.lnRawOrderLog1[0]!=C_SPOT)&&(data.lnRawOrderLog2[0]!=C_SPOT))
lilac0112_1 0:b910276f9da2 538
lilac0112_1 0:b910276f9da2 539 ){
lilac0112_1 0:b910276f9da2 540 *y = -LineReturn[4];
lilac0112_1 0:b910276f9da2 541 data.lnStop[Y_LINE]=0;//JSO2
lilac0112_1 0:b910276f9da2 542 }
lilac0112_1 0:b910276f9da2 543 }
lilac0112_1 0:b910276f9da2 544 else{
lilac0112_1 0:b910276f9da2 545 lnRawReturn1=0;
lilac0112_1 0:b910276f9da2 546
lilac0112_1 0:b910276f9da2 547
lilac0112_1 0:b910276f9da2 548 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 0:b910276f9da2 549 *x = -LineReturn[4];
lilac0112_1 0:b910276f9da2 550 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 551 }
lilac0112_1 0:b910276f9da2 552 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 0:b910276f9da2 553 *x = LineReturn[4];
lilac0112_1 0:b910276f9da2 554 data.lnStop[X_LINE]=0;
lilac0112_1 0:b910276f9da2 555 }
lilac0112_1 0:b910276f9da2 556
lilac0112_1 0:b910276f9da2 557 }
lilac0112_1 0:b910276f9da2 558
lilac0112_1 0:b910276f9da2 559 }
lilac0112_1 0:b910276f9da2 560 }
lilac0112_1 0:b910276f9da2 561 void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 562 //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 0:b910276f9da2 563 //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 0:b910276f9da2 564 static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 0:b910276f9da2 565 static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 0:b910276f9da2 566
lilac0112_1 0:b910276f9da2 567 if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){
lilac0112_1 0:b910276f9da2 568 if(data.FieldSpot==LINE_INSIDE){
lilac0112_1 0:b910276f9da2 569
lilac0112_1 0:b910276f9da2 570 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 0:b910276f9da2 571 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 0:b910276f9da2 572 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 573 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 574 if(
lilac0112_1 0:b910276f9da2 575 (data.lnCorner[L_LINE])||
lilac0112_1 0:b910276f9da2 576 (data.lnCorner[R_LINE])||
lilac0112_1 0:b910276f9da2 577 (data.lnCorner[F_LINE])||
lilac0112_1 0:b910276f9da2 578 (data.lnCorner[B_LINE])
lilac0112_1 0:b910276f9da2 579 ){
lilac0112_1 0:b910276f9da2 580
lilac0112_1 0:b910276f9da2 581 data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
lilac0112_1 0:b910276f9da2 582 data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
lilac0112_1 0:b910276f9da2 583 data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 584 data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
lilac0112_1 0:b910276f9da2 585
lilac0112_1 0:b910276f9da2 586
lilac0112_1 0:b910276f9da2 587
lilac0112_1 0:b910276f9da2 588 ///*
lilac0112_1 0:b910276f9da2 589 LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
lilac0112_1 0:b910276f9da2 590 LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
lilac0112_1 0:b910276f9da2 591 LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
lilac0112_1 0:b910276f9da2 592 LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
lilac0112_1 0:b910276f9da2 593
lilac0112_1 0:b910276f9da2 594 NewLineCorner[L_LINE]=data.lnCorner[L_LINE];
lilac0112_1 0:b910276f9da2 595 NewLineCorner[R_LINE]=data.lnCorner[R_LINE];
lilac0112_1 0:b910276f9da2 596 NewLineCorner[F_LINE]=data.lnCorner[F_LINE];
lilac0112_1 0:b910276f9da2 597 NewLineCorner[B_LINE]=data.lnCorner[B_LINE];
lilac0112_1 0:b910276f9da2 598
lilac0112_1 0:b910276f9da2 599 if(
lilac0112_1 0:b910276f9da2 600 (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&&
lilac0112_1 0:b910276f9da2 601 (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&&
lilac0112_1 0:b910276f9da2 602 (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&&
lilac0112_1 0:b910276f9da2 603 (LastLineCorner[B_LINE]==NewLineCorner[B_LINE])
lilac0112_1 0:b910276f9da2 604 ){
lilac0112_1 0:b910276f9da2 605 data.lnRepeat++;
lilac0112_1 0:b910276f9da2 606 }
lilac0112_1 0:b910276f9da2 607 else{
lilac0112_1 0:b910276f9da2 608 data.lnRepeat=0;
lilac0112_1 0:b910276f9da2 609 }
lilac0112_1 0:b910276f9da2 610 //*/
lilac0112_1 0:b910276f9da2 611
lilac0112_1 0:b910276f9da2 612 data.FieldSpot = LINE_OUTSIDE;
lilac0112_1 0:b910276f9da2 613 LineLiberate();
lilac0112_1 0:b910276f9da2 614 }
lilac0112_1 0:b910276f9da2 615 }
lilac0112_1 0:b910276f9da2 616 else if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 0:b910276f9da2 617 if(data.lnRaw==0){
lilac0112_1 0:b910276f9da2 618 data.FieldSpot = LINE_INSIDE;
lilac0112_1 0:b910276f9da2 619 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 0:b910276f9da2 620 LineLiberate();
lilac0112_1 0:b910276f9da2 621 LineRankClear();
lilac0112_1 0:b910276f9da2 622 }
lilac0112_1 0:b910276f9da2 623 }
lilac0112_1 0:b910276f9da2 624 }
lilac0112_1 0:b910276f9da2 625 if(data.FieldSpot == LINE_OUTSIDE){
lilac0112_1 0:b910276f9da2 626 if(
lilac0112_1 0:b910276f9da2 627 (
lilac0112_1 0:b910276f9da2 628 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 0:b910276f9da2 629 (data.NonWall[L_LINE]==1)||
lilac0112_1 0:b910276f9da2 630 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 0:b910276f9da2 631 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 0:b910276f9da2 632 )&&
lilac0112_1 0:b910276f9da2 633 (
lilac0112_1 0:b910276f9da2 634 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 0:b910276f9da2 635 (data.NonWall[R_LINE]==1)||
lilac0112_1 0:b910276f9da2 636 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 0:b910276f9da2 637 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 0:b910276f9da2 638 )&&
lilac0112_1 0:b910276f9da2 639 (
lilac0112_1 0:b910276f9da2 640 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 0:b910276f9da2 641 (data.NonWall[F_LINE]==1)||
lilac0112_1 0:b910276f9da2 642 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 0:b910276f9da2 643 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 0:b910276f9da2 644 )&&
lilac0112_1 0:b910276f9da2 645 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 0:b910276f9da2 646 (data.NonWall[B_LINE]==1)||
lilac0112_1 0:b910276f9da2 647 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 0:b910276f9da2 648 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 0:b910276f9da2 649 )
lilac0112_1 0:b910276f9da2 650 ){
lilac0112_1 0:b910276f9da2 651 data.FieldSpot = LINE_INSIDE;
lilac0112_1 0:b910276f9da2 652 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 0:b910276f9da2 653 LineLiberate();
lilac0112_1 0:b910276f9da2 654 LineRankClear();
lilac0112_1 0:b910276f9da2 655 }
lilac0112_1 0:b910276f9da2 656 }
lilac0112_1 0:b910276f9da2 657 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
lilac0112_1 0:b910276f9da2 658 if(
lilac0112_1 0:b910276f9da2 659 (
lilac0112_1 0:b910276f9da2 660 (
lilac0112_1 0:b910276f9da2 661 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 0:b910276f9da2 662 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 0:b910276f9da2 663 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 0:b910276f9da2 664 )&&
lilac0112_1 0:b910276f9da2 665 (
lilac0112_1 0:b910276f9da2 666 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 0:b910276f9da2 667 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 0:b910276f9da2 668 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 0:b910276f9da2 669 )&&
lilac0112_1 0:b910276f9da2 670 (
lilac0112_1 0:b910276f9da2 671 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 0:b910276f9da2 672 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 0:b910276f9da2 673 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 0:b910276f9da2 674 )&&
lilac0112_1 0:b910276f9da2 675 (
lilac0112_1 0:b910276f9da2 676 (data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 0:b910276f9da2 677 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 0:b910276f9da2 678 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 0:b910276f9da2 679 )
lilac0112_1 0:b910276f9da2 680 )
lilac0112_1 0:b910276f9da2 681 ||
lilac0112_1 0:b910276f9da2 682 (
lilac0112_1 0:b910276f9da2 683 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 0:b910276f9da2 684 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 0:b910276f9da2 685 )
lilac0112_1 0:b910276f9da2 686 ){
lilac0112_1 0:b910276f9da2 687 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 0:b910276f9da2 688 LineLiberate();
lilac0112_1 0:b910276f9da2 689 }
lilac0112_1 0:b910276f9da2 690 }
lilac0112_1 0:b910276f9da2 691 ///*
lilac0112_1 0:b910276f9da2 692 if(
lilac0112_1 0:b910276f9da2 693 (data.irNotice==IR_NONE)||
lilac0112_1 0:b910276f9da2 694 (data.irNotice==IR_FAR)||
lilac0112_1 0:b910276f9da2 695 (
lilac0112_1 0:b910276f9da2 696 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 0:b910276f9da2 697 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 0:b910276f9da2 698 )
lilac0112_1 0:b910276f9da2 699 ){
lilac0112_1 0:b910276f9da2 700 data.lnRepeat = 0;
lilac0112_1 0:b910276f9da2 701
lilac0112_1 0:b910276f9da2 702 NewLineCorner[L_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 703 NewLineCorner[R_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 704 NewLineCorner[F_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 705 NewLineCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 706
lilac0112_1 0:b910276f9da2 707 LastLineCorner[L_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 708 LastLineCorner[R_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 709 LastLineCorner[F_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 710 LastLineCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 0:b910276f9da2 711 }
lilac0112_1 0:b910276f9da2 712 //data.lnRepeat=0;
lilac0112_1 0:b910276f9da2 713 if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){
lilac0112_1 0:b910276f9da2 714 //x
lilac0112_1 0:b910276f9da2 715 if(
lilac0112_1 0:b910276f9da2 716 ((pow_x>=0)&&(NewLineCorner[R_LINE]))||
lilac0112_1 0:b910276f9da2 717 ((pow_x<0)&&(NewLineCorner[L_LINE]))
lilac0112_1 0:b910276f9da2 718 ){
lilac0112_1 0:b910276f9da2 719 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 720 }
lilac0112_1 0:b910276f9da2 721 else{
lilac0112_1 0:b910276f9da2 722 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 723 }
lilac0112_1 0:b910276f9da2 724 //y
lilac0112_1 0:b910276f9da2 725 if(
lilac0112_1 0:b910276f9da2 726 ((pow_y>=0)&&(NewLineCorner[F_LINE]))||
lilac0112_1 0:b910276f9da2 727 ((pow_y<0)&&(NewLineCorner[B_LINE]))
lilac0112_1 0:b910276f9da2 728 ){
lilac0112_1 0:b910276f9da2 729 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 730 }
lilac0112_1 0:b910276f9da2 731 else{
lilac0112_1 0:b910276f9da2 732 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 733 }
lilac0112_1 0:b910276f9da2 734 }
lilac0112_1 0:b910276f9da2 735 else{
lilac0112_1 0:b910276f9da2 736 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 737 }
lilac0112_1 0:b910276f9da2 738 //*/
lilac0112_1 0:b910276f9da2 739
lilac0112_1 0:b910276f9da2 740 }
lilac0112_1 0:b910276f9da2 741 void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 742
lilac0112_1 0:b910276f9da2 743 if(data.lnRepeat==0){
lilac0112_1 0:b910276f9da2 744 data.lnStayNow[X_LINE]=0;
lilac0112_1 0:b910276f9da2 745 data.lnStayNow[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 746 }
lilac0112_1 0:b910276f9da2 747 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 748 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 749
lilac0112_1 0:b910276f9da2 750 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
lilac0112_1 0:b910276f9da2 751 data.lnRepeat=1;
lilac0112_1 0:b910276f9da2 752 }
lilac0112_1 0:b910276f9da2 753
lilac0112_1 0:b910276f9da2 754 if(
lilac0112_1 0:b910276f9da2 755 (data.irNotice==IR_NONE)||
lilac0112_1 0:b910276f9da2 756 //(data.irNotice==IR_FAR)||
lilac0112_1 0:b910276f9da2 757 //(data.irLastPosition<=7)||
lilac0112_1 0:b910276f9da2 758 (
lilac0112_1 0:b910276f9da2 759 (!(
lilac0112_1 0:b910276f9da2 760 (data.irPosition==data.irLastPosition)||
lilac0112_1 0:b910276f9da2 761 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
lilac0112_1 0:b910276f9da2 762 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))||
lilac0112_1 0:b910276f9da2 763 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
lilac0112_1 0:b910276f9da2 764 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
lilac0112_1 0:b910276f9da2 765 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
lilac0112_1 0:b910276f9da2 766 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
lilac0112_1 0:b910276f9da2 767 ))&&(
lilac0112_1 0:b910276f9da2 768 (data.irLastNotice==IR_CLOSE)||
lilac0112_1 0:b910276f9da2 769 (data.irLastNotice==IR_CLOSER)
lilac0112_1 0:b910276f9da2 770 )
lilac0112_1 0:b910276f9da2 771 )
lilac0112_1 0:b910276f9da2 772 ){
lilac0112_1 0:b910276f9da2 773 data.lnRepeat = 0;
lilac0112_1 0:b910276f9da2 774
lilac0112_1 0:b910276f9da2 775 LineRawLogReset();
lilac0112_1 0:b910276f9da2 776 }
lilac0112_1 0:b910276f9da2 777 //data.lnRepeat=0;
lilac0112_1 0:b910276f9da2 778 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
lilac0112_1 0:b910276f9da2 779 if(
lilac0112_1 0:b910276f9da2 780 (data.lnRepeat>=1)&&
lilac0112_1 0:b910276f9da2 781 (data.lnRawOrder[0]==LINE_EMPTY)/*&&
lilac0112_1 0:b910276f9da2 782 (
lilac0112_1 0:b910276f9da2 783 (
lilac0112_1 0:b910276f9da2 784 (data.irPosition==data.irLastPosition)||
lilac0112_1 0:b910276f9da2 785 (data.irPosition==(data.irLastPosition+1))||
lilac0112_1 0:b910276f9da2 786 (data.irPosition==(data.irLastPosition-1))||
lilac0112_1 0:b910276f9da2 787 (data.irPosition==(data.irLastPosition+11))||
lilac0112_1 0:b910276f9da2 788 (data.irPosition==(data.irLastPosition-11))
lilac0112_1 0:b910276f9da2 789 )&&
lilac0112_1 0:b910276f9da2 790 (
lilac0112_1 0:b910276f9da2 791 (data.irNotice==IR_CLOSE)||
lilac0112_1 0:b910276f9da2 792 (data.irNotice==IR_CLOSER)
lilac0112_1 0:b910276f9da2 793 )
lilac0112_1 0:b910276f9da2 794 )*/
lilac0112_1 0:b910276f9da2 795 ){
lilac0112_1 0:b910276f9da2 796 //y
lilac0112_1 0:b910276f9da2 797 if(
lilac0112_1 0:b910276f9da2 798 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
lilac0112_1 0:b910276f9da2 799 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 800 ){
lilac0112_1 0:b910276f9da2 801 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 802 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 803 }
lilac0112_1 0:b910276f9da2 804 else{
lilac0112_1 0:b910276f9da2 805 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 806 }
lilac0112_1 0:b910276f9da2 807 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 808 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 809 }
lilac0112_1 0:b910276f9da2 810 else{
lilac0112_1 0:b910276f9da2 811 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 812 }
lilac0112_1 0:b910276f9da2 813 if(
lilac0112_1 0:b910276f9da2 814 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
lilac0112_1 0:b910276f9da2 815 ){
lilac0112_1 0:b910276f9da2 816 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 817 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 818 }
lilac0112_1 0:b910276f9da2 819 else{
lilac0112_1 0:b910276f9da2 820 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 821 }
lilac0112_1 0:b910276f9da2 822 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 823 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 824 }
lilac0112_1 0:b910276f9da2 825 else{
lilac0112_1 0:b910276f9da2 826 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 827 }
lilac0112_1 0:b910276f9da2 828 //x
lilac0112_1 0:b910276f9da2 829 if(
lilac0112_1 0:b910276f9da2 830 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 831 ){
lilac0112_1 0:b910276f9da2 832 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 833 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 834 }
lilac0112_1 0:b910276f9da2 835 else{
lilac0112_1 0:b910276f9da2 836 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 837 }
lilac0112_1 0:b910276f9da2 838 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 839 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 840 }
lilac0112_1 0:b910276f9da2 841 else{
lilac0112_1 0:b910276f9da2 842 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 843 }
lilac0112_1 0:b910276f9da2 844 if(
lilac0112_1 0:b910276f9da2 845 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 846 ){
lilac0112_1 0:b910276f9da2 847 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 848 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 849 }
lilac0112_1 0:b910276f9da2 850 else{
lilac0112_1 0:b910276f9da2 851 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 852 }
lilac0112_1 0:b910276f9da2 853 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 854 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 855 }
lilac0112_1 0:b910276f9da2 856 else{
lilac0112_1 0:b910276f9da2 857 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 858 }
lilac0112_1 0:b910276f9da2 859 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
lilac0112_1 0:b910276f9da2 860 //y2
lilac0112_1 0:b910276f9da2 861 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 0:b910276f9da2 862 if(
lilac0112_1 0:b910276f9da2 863 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 864 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 865 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19)))
lilac0112_1 0:b910276f9da2 866 ){
lilac0112_1 0:b910276f9da2 867 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 868 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 869 }
lilac0112_1 0:b910276f9da2 870 else{
lilac0112_1 0:b910276f9da2 871 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 872 }
lilac0112_1 0:b910276f9da2 873 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 874 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 875 }
lilac0112_1 0:b910276f9da2 876 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 877 }
lilac0112_1 0:b910276f9da2 878 else{
lilac0112_1 0:b910276f9da2 879 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 880 }
lilac0112_1 0:b910276f9da2 881 }
lilac0112_1 0:b910276f9da2 882 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 0:b910276f9da2 883 if(
lilac0112_1 0:b910276f9da2 884 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 0:b910276f9da2 885 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 0:b910276f9da2 886 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 0:b910276f9da2 887 ){
lilac0112_1 0:b910276f9da2 888 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 889 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 890 }
lilac0112_1 0:b910276f9da2 891 else{
lilac0112_1 0:b910276f9da2 892 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 893 }
lilac0112_1 0:b910276f9da2 894 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 895 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 896 }
lilac0112_1 0:b910276f9da2 897 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 898 }
lilac0112_1 0:b910276f9da2 899 else{
lilac0112_1 0:b910276f9da2 900 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 901 }
lilac0112_1 0:b910276f9da2 902 }
lilac0112_1 0:b910276f9da2 903 //x2
lilac0112_1 0:b910276f9da2 904 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
lilac0112_1 0:b910276f9da2 905 if(
lilac0112_1 0:b910276f9da2 906 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 907 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
lilac0112_1 0:b910276f9da2 908 ){
lilac0112_1 0:b910276f9da2 909 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 910 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 911 }
lilac0112_1 0:b910276f9da2 912 else{
lilac0112_1 0:b910276f9da2 913 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 914 }
lilac0112_1 0:b910276f9da2 915 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 916 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 917 }
lilac0112_1 0:b910276f9da2 918 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 919 }
lilac0112_1 0:b910276f9da2 920 else{
lilac0112_1 0:b910276f9da2 921 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 922 }
lilac0112_1 0:b910276f9da2 923
lilac0112_1 0:b910276f9da2 924 if(
lilac0112_1 0:b910276f9da2 925 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
lilac0112_1 0:b910276f9da2 926 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 0:b910276f9da2 927 ){
lilac0112_1 0:b910276f9da2 928 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 929 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 930 }
lilac0112_1 0:b910276f9da2 931 else{
lilac0112_1 0:b910276f9da2 932 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 933 }
lilac0112_1 0:b910276f9da2 934 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 935 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 936 }
lilac0112_1 0:b910276f9da2 937 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 938 }
lilac0112_1 0:b910276f9da2 939 else{
lilac0112_1 0:b910276f9da2 940 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 941 }
lilac0112_1 0:b910276f9da2 942 }
lilac0112_1 0:b910276f9da2 943 if(
lilac0112_1 0:b910276f9da2 944 (data.lnRepeat>=2)
lilac0112_1 0:b910276f9da2 945 ){
lilac0112_1 0:b910276f9da2 946 if(
lilac0112_1 0:b910276f9da2 947 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
lilac0112_1 0:b910276f9da2 948 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
lilac0112_1 0:b910276f9da2 949 ){
lilac0112_1 0:b910276f9da2 950 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 951 }
lilac0112_1 0:b910276f9da2 952 if(
lilac0112_1 0:b910276f9da2 953 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
lilac0112_1 0:b910276f9da2 954 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
lilac0112_1 0:b910276f9da2 955 ){
lilac0112_1 0:b910276f9da2 956 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 957 }
lilac0112_1 0:b910276f9da2 958 }
lilac0112_1 0:b910276f9da2 959 }
lilac0112_1 0:b910276f9da2 960
lilac0112_1 0:b910276f9da2 961
lilac0112_1 0:b910276f9da2 962 }
lilac0112_1 0:b910276f9da2 963 else{
lilac0112_1 0:b910276f9da2 964 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 965 }
lilac0112_1 0:b910276f9da2 966
lilac0112_1 0:b910276f9da2 967 }
lilac0112_1 0:b910276f9da2 968 void LineJudgeReset3(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 0:b910276f9da2 969
lilac0112_1 0:b910276f9da2 970 if(data.lnRepeat==0){
lilac0112_1 0:b910276f9da2 971 data.lnStayNow[X_LINE]=0;
lilac0112_1 0:b910276f9da2 972 data.lnStayNow[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 973 }
lilac0112_1 0:b910276f9da2 974 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 975 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 976
lilac0112_1 0:b910276f9da2 977 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
lilac0112_1 0:b910276f9da2 978 data.lnRepeat=1;
lilac0112_1 0:b910276f9da2 979 }
lilac0112_1 0:b910276f9da2 980
lilac0112_1 0:b910276f9da2 981 if(
lilac0112_1 0:b910276f9da2 982 (data.irNotice==IR_NONE)||
lilac0112_1 0:b910276f9da2 983 (data.irNotice==IR_FAR)||
lilac0112_1 0:b910276f9da2 984 //(data.irLastPosition<=7)||
lilac0112_1 0:b910276f9da2 985 (
lilac0112_1 0:b910276f9da2 986 (!(
lilac0112_1 0:b910276f9da2 987 (data.irPosition==data.irLastPosition)||
lilac0112_1 0:b910276f9da2 988 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
lilac0112_1 0:b910276f9da2 989 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))
lilac0112_1 0:b910276f9da2 990 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
lilac0112_1 0:b910276f9da2 991 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
lilac0112_1 0:b910276f9da2 992 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
lilac0112_1 0:b910276f9da2 993 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
lilac0112_1 0:b910276f9da2 994 ))&&(
lilac0112_1 0:b910276f9da2 995 (data.irLastNotice==IR_CLOSE)||
lilac0112_1 0:b910276f9da2 996 (data.irLastNotice==IR_CLOSER)
lilac0112_1 0:b910276f9da2 997 )
lilac0112_1 0:b910276f9da2 998 )
lilac0112_1 0:b910276f9da2 999 ){
lilac0112_1 0:b910276f9da2 1000 data.lnRepeat = 0;
lilac0112_1 0:b910276f9da2 1001
lilac0112_1 0:b910276f9da2 1002 LineRawLogReset();
lilac0112_1 0:b910276f9da2 1003 }
lilac0112_1 0:b910276f9da2 1004 //data.lnRepeat=0;
lilac0112_1 0:b910276f9da2 1005 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
lilac0112_1 0:b910276f9da2 1006 if(
lilac0112_1 0:b910276f9da2 1007 (data.lnRepeat>=1)&&
lilac0112_1 0:b910276f9da2 1008 (data.lnRawOrder[0]==LINE_EMPTY)/*&&
lilac0112_1 0:b910276f9da2 1009 (
lilac0112_1 0:b910276f9da2 1010 (
lilac0112_1 0:b910276f9da2 1011 (data.irPosition==data.irLastPosition)||
lilac0112_1 0:b910276f9da2 1012 (data.irPosition==(data.irLastPosition+1))||
lilac0112_1 0:b910276f9da2 1013 (data.irPosition==(data.irLastPosition-1))||
lilac0112_1 0:b910276f9da2 1014 (data.irPosition==(data.irLastPosition+11))||
lilac0112_1 0:b910276f9da2 1015 (data.irPosition==(data.irLastPosition-11))
lilac0112_1 0:b910276f9da2 1016 )&&
lilac0112_1 0:b910276f9da2 1017 (
lilac0112_1 0:b910276f9da2 1018 (data.irNotice==IR_CLOSE)||
lilac0112_1 0:b910276f9da2 1019 (data.irNotice==IR_CLOSER)
lilac0112_1 0:b910276f9da2 1020 )
lilac0112_1 0:b910276f9da2 1021 )*/
lilac0112_1 0:b910276f9da2 1022 ){
lilac0112_1 0:b910276f9da2 1023 //y
lilac0112_1 0:b910276f9da2 1024 if(
lilac0112_1 0:b910276f9da2 1025 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
lilac0112_1 0:b910276f9da2 1026 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 1027 ){
lilac0112_1 0:b910276f9da2 1028 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 1029 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1030 }
lilac0112_1 0:b910276f9da2 1031 else{
lilac0112_1 0:b910276f9da2 1032 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1033 }
lilac0112_1 0:b910276f9da2 1034 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1035 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1036 }
lilac0112_1 0:b910276f9da2 1037 else{
lilac0112_1 0:b910276f9da2 1038 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1039 }
lilac0112_1 0:b910276f9da2 1040 if(
lilac0112_1 0:b910276f9da2 1041 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
lilac0112_1 0:b910276f9da2 1042 ){
lilac0112_1 0:b910276f9da2 1043 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 1044 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1045 }
lilac0112_1 0:b910276f9da2 1046 else{
lilac0112_1 0:b910276f9da2 1047 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1048 }
lilac0112_1 0:b910276f9da2 1049 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1050 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1051 }
lilac0112_1 0:b910276f9da2 1052 else{
lilac0112_1 0:b910276f9da2 1053 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1054 }
lilac0112_1 0:b910276f9da2 1055 //x
lilac0112_1 0:b910276f9da2 1056 if(
lilac0112_1 0:b910276f9da2 1057 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 1058 ){
lilac0112_1 0:b910276f9da2 1059 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 1060 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1061 }
lilac0112_1 0:b910276f9da2 1062 else{
lilac0112_1 0:b910276f9da2 1063 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1064 }
lilac0112_1 0:b910276f9da2 1065 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1066 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1067 }
lilac0112_1 0:b910276f9da2 1068 else{
lilac0112_1 0:b910276f9da2 1069 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1070 }
lilac0112_1 0:b910276f9da2 1071 if(
lilac0112_1 0:b910276f9da2 1072 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 0:b910276f9da2 1073 ){
lilac0112_1 0:b910276f9da2 1074 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 1075 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1076 }
lilac0112_1 0:b910276f9da2 1077 else{
lilac0112_1 0:b910276f9da2 1078 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1079 }
lilac0112_1 0:b910276f9da2 1080 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1081 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1082 }
lilac0112_1 0:b910276f9da2 1083 else{
lilac0112_1 0:b910276f9da2 1084 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1085 }
lilac0112_1 0:b910276f9da2 1086 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
lilac0112_1 0:b910276f9da2 1087 //y2
lilac0112_1 0:b910276f9da2 1088 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 0:b910276f9da2 1089 if(
lilac0112_1 0:b910276f9da2 1090 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 1091 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 1092 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19)))
lilac0112_1 0:b910276f9da2 1093 ){
lilac0112_1 0:b910276f9da2 1094 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 1095 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1096 }
lilac0112_1 0:b910276f9da2 1097 else{
lilac0112_1 0:b910276f9da2 1098 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1099 }
lilac0112_1 0:b910276f9da2 1100 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 1101 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1102 }
lilac0112_1 0:b910276f9da2 1103 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1104 }
lilac0112_1 0:b910276f9da2 1105 else{
lilac0112_1 0:b910276f9da2 1106 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1107 }
lilac0112_1 0:b910276f9da2 1108 }
lilac0112_1 0:b910276f9da2 1109 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 0:b910276f9da2 1110 if(
lilac0112_1 0:b910276f9da2 1111 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 0:b910276f9da2 1112 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 0:b910276f9da2 1113 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 0:b910276f9da2 1114 ){
lilac0112_1 0:b910276f9da2 1115 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 0:b910276f9da2 1116 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1117 }
lilac0112_1 0:b910276f9da2 1118 else{
lilac0112_1 0:b910276f9da2 1119 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1120 }
lilac0112_1 0:b910276f9da2 1121 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 1122 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1123 }
lilac0112_1 0:b910276f9da2 1124 data.lnStayNow[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1125 }
lilac0112_1 0:b910276f9da2 1126 else{
lilac0112_1 0:b910276f9da2 1127 data.lnStay[X_LINE]=1;
lilac0112_1 0:b910276f9da2 1128 }
lilac0112_1 0:b910276f9da2 1129 }
lilac0112_1 0:b910276f9da2 1130 //x2
lilac0112_1 0:b910276f9da2 1131 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
lilac0112_1 0:b910276f9da2 1132 if(
lilac0112_1 0:b910276f9da2 1133 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 0:b910276f9da2 1134 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
lilac0112_1 0:b910276f9da2 1135 ){
lilac0112_1 0:b910276f9da2 1136 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 1137 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1138 }
lilac0112_1 0:b910276f9da2 1139 else{
lilac0112_1 0:b910276f9da2 1140 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1141 }
lilac0112_1 0:b910276f9da2 1142 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 1143 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1144 }
lilac0112_1 0:b910276f9da2 1145 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1146 }
lilac0112_1 0:b910276f9da2 1147 else{
lilac0112_1 0:b910276f9da2 1148 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1149 }
lilac0112_1 0:b910276f9da2 1150
lilac0112_1 0:b910276f9da2 1151 if(
lilac0112_1 0:b910276f9da2 1152 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
lilac0112_1 0:b910276f9da2 1153 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 0:b910276f9da2 1154 ){
lilac0112_1 0:b910276f9da2 1155 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 0:b910276f9da2 1156 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1157 }
lilac0112_1 0:b910276f9da2 1158 else{
lilac0112_1 0:b910276f9da2 1159 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1160 }
lilac0112_1 0:b910276f9da2 1161 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 0:b910276f9da2 1162 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1163 }
lilac0112_1 0:b910276f9da2 1164 data.lnStayNow[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1165 }
lilac0112_1 0:b910276f9da2 1166 else{
lilac0112_1 0:b910276f9da2 1167 data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1168 }
lilac0112_1 0:b910276f9da2 1169 }
lilac0112_1 0:b910276f9da2 1170 if(
lilac0112_1 0:b910276f9da2 1171 (data.lnRepeat>=2)
lilac0112_1 0:b910276f9da2 1172 ){
lilac0112_1 0:b910276f9da2 1173 if(
lilac0112_1 0:b910276f9da2 1174 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
lilac0112_1 0:b910276f9da2 1175 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
lilac0112_1 0:b910276f9da2 1176 ){
lilac0112_1 0:b910276f9da2 1177 data.lnStay[Y_LINE]=0;
lilac0112_1 0:b910276f9da2 1178 }
lilac0112_1 0:b910276f9da2 1179 if(
lilac0112_1 0:b910276f9da2 1180 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
lilac0112_1 0:b910276f9da2 1181 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
lilac0112_1 0:b910276f9da2 1182 ){
lilac0112_1 0:b910276f9da2 1183 data.lnStay[X_LINE]=0;
lilac0112_1 0:b910276f9da2 1184 }
lilac0112_1 0:b910276f9da2 1185 }
lilac0112_1 0:b910276f9da2 1186 }
lilac0112_1 0:b910276f9da2 1187
lilac0112_1 0:b910276f9da2 1188
lilac0112_1 0:b910276f9da2 1189 }
lilac0112_1 0:b910276f9da2 1190 else{
lilac0112_1 0:b910276f9da2 1191 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 0:b910276f9da2 1192 }
lilac0112_1 0:b910276f9da2 1193
lilac0112_1 0:b910276f9da2 1194 }
lilac0112_1 0:b910276f9da2 1195 void JudgeInSide(void){
lilac0112_1 0:b910276f9da2 1196 data.FieldSpot = LINE_INSIDE;
lilac0112_1 0:b910276f9da2 1197 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 0:b910276f9da2 1198 LineLiberate();
lilac0112_1 0:b910276f9da2 1199 LineRankClear();
lilac0112_1 0:b910276f9da2 1200 //Line_home.detach();
lilac0112_1 0:b910276f9da2 1201 }