CatPot 2015-2016
ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Embed:
(wiki syntax)
Show/hide line numbers
LineProcess.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){ 00005 uint8_t LineState; 00006 uint8_t LineSum; 00007 00008 //line 00009 LineState = 0; 00010 00011 LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1); 00012 if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){ 00013 LineState = 3; 00014 } 00015 else if(LineSum==2){ 00016 LineState = 2; 00017 } 00018 else if(LineSum==1){ 00019 LineState = 1; 00020 } 00021 else if(LineSum==0){ 00022 LineState = 0; 00023 } 00024 00025 00026 if(pow_x>=0){ 00027 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; 00028 else *x=1; 00029 } 00030 else{ 00031 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; 00032 else *x=1; 00033 } 00034 00035 if(pow_y>=0){ 00036 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; 00037 else *y=1; 00038 } 00039 else{ 00040 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; 00041 else *y=1; 00042 } 00043 } 00044 void LineJudgeSlow2(double pow_x, double pow_y, double *x, double *y, uint8_t *px, uint8_t *py){ 00045 uint8_t LineState; 00046 uint8_t LineSum; 00047 00048 //line 00049 LineState = 0; 00050 00051 LineSum = (data.lnRawOrder[0]!=LINE_EMPTY)+(data.lnRawOrder[1]!=LINE_EMPTY)+(data.lnRawOrder[2]!=LINE_EMPTY); 00052 if(LineSum==3){ 00053 LineState = 3; 00054 } 00055 else if(LineSum==2){ 00056 LineState = 2; 00057 } 00058 else if(LineSum==1){ 00059 LineState = 1; 00060 } 00061 else if(LineSum==0){ 00062 LineState = 0; 00063 } 00064 00065 if(LineSum>0){ 00066 if(pow_x>=0){ 00067 if((data.ping[R_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState]; 00068 } 00069 else{ 00070 if((data.ping[L_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState]; 00071 } 00072 00073 if(pow_y>=0){ 00074 if((data.ping[F_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState]; 00075 } 00076 else{ 00077 if((data.ping[B_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState]; 00078 } 00079 } 00080 else if(LineSum==0){ 00081 ////超音波による減速 00082 if(pow_x>=0){ 00083 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState]; 00084 } 00085 else{ 00086 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState]; 00087 } 00088 00089 if(pow_y>=0){ 00090 //if((data.ping[F_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState]; 00091 } 00092 else{ 00093 if((data.ping[B_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState]; 00094 } 00095 } 00096 00097 if((data.lnStayNow[X_LINE]==1)&&((*px)>15)){ 00098 (*px)=15; 00099 } 00100 if((data.lnStayNow[Y_LINE]==1)&&((*py)>15)){ 00101 (*py)=15; 00102 } 00103 00104 ////超音波による減速 00105 if(pow_x>=0){ 00106 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){ 00107 (*px)=20; 00108 if((*py)>30) (*py)=30; 00109 } 00110 } 00111 else{ 00112 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){ 00113 (*px)=20; 00114 if((*py)>30) (*py)=30; 00115 } 00116 } 00117 00118 if(pow_y>=0){ 00119 if((data.ping[F_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25; 00120 } 00121 else{ 00122 if((data.ping[B_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25; 00123 } 00124 00125 *x=1; 00126 *y=1; 00127 } 00128 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){ 00129 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20}; 00130 uint8_t LinePingState[4]; 00131 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される． 00132 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される． 00133 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する． 00134 //×ラインのほぼ場外では常時ラインから離れる力が優先される． 00135 //※場外判定を行うには再び場内に戻る必要がある． 00136 00137 if(data.FieldSpot==LINE_OUTSIDE){ 00138 00139 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]); 00140 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]); 00141 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]); 00142 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]); 00143 00144 //line間際の復帰力以外の力を作用させるか否か 00145 data.lnStop[X_LINE]=1; 00146 data.lnStop[Y_LINE]=1; 00147 00148 data.ReturnDir[X_LINE]=LINE_EMPTY; 00149 data.ReturnDir[Y_LINE]=LINE_EMPTY; 00150 00151 //x 00152 if(data.ReturnDir[X_LINE]==L_LINE){ 00153 if(pow_x<0){ 00154 *x = 0; 00155 data.lnStop[X_LINE]=1; 00156 } 00157 else{ 00158 *x = -LineReturn[4]; 00159 data.lnStop[X_LINE]=0; 00160 } 00161 } 00162 else if(data.ReturnDir[X_LINE]==R_LINE){ 00163 if(pow_x>0){ 00164 *x = 0; 00165 data.lnStop[X_LINE]=1; 00166 } 00167 else{ 00168 *x = LineReturn[4]; 00169 data.lnStop[X_LINE]=0; 00170 } 00171 } 00172 else if(data.ReturnDir[X_LINE]==LINE_EMPTY){ 00173 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){ 00174 /*if(pow_x>0){ 00175 *x = -pow_x*(1-0.75); 00176 } 00177 else{ 00178 *x = pow_x*(1-0.75); 00179 }*/ 00180 data.lnStop[X_LINE]=1; 00181 } 00182 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ 00183 00184 data.ReturnDir[X_LINE]=L_LINE; 00185 00186 /*if(pow_x<0){ 00187 *x = 0; 00188 data.lnStop[X_LINE]=1; 00189 } 00190 else{ 00191 *x = -LineReturn[4]; 00192 data.lnStop[X_LINE]=0; 00193 }*/ 00194 *x = -LineReturn[4]; 00195 data.lnStop[X_LINE]=0; 00196 } 00197 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ 00198 00199 data.ReturnDir[X_LINE]=R_LINE; 00200 00201 /*if(pow_x>0){ 00202 *x = 0; 00203 data.lnStop[X_LINE]=1; 00204 } 00205 else{ 00206 *x = LineReturn[4]; 00207 data.lnStop[X_LINE]=0; 00208 }*/ 00209 *x = LineReturn[4]; 00210 data.lnStop[X_LINE]=0; 00211 } 00212 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){ 00213 00214 if( 00215 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==B_SPOT) 00216 ){ 00217 *x = -LineReturn[4]; 00218 data.lnStop[X_LINE]=0; 00219 } 00220 else if( 00221 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==A_SPOT) 00222 ){ 00223 *x = LineReturn[4]; 00224 data.lnStop[X_LINE]=0; 00225 } 00226 else{ 00227 *x = 0; 00228 data.lnStop[X_LINE]=0; 00229 } 00230 } 00231 } 00232 //y 00233 if(data.ReturnDir[Y_LINE]==F_LINE){ 00234 if(pow_y>0){ 00235 *y = 0; 00236 data.lnStop[Y_LINE]=1; 00237 } 00238 else{ 00239 *y = LineReturn[4]; 00240 data.lnStop[Y_LINE]=0; 00241 } 00242 } 00243 else if(data.ReturnDir[Y_LINE]==B_LINE){ 00244 if(pow_y<0){ 00245 *y = 0; 00246 data.lnStop[Y_LINE]=1; 00247 } 00248 else{ 00249 *y = -LineReturn[4]; 00250 data.lnStop[Y_LINE]=0; 00251 } 00252 } 00253 else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){ 00254 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){ 00255 /*if(pow_y>0){ 00256 *y = -pow_y*(1-0.75); 00257 } 00258 else{ 00259 *y = pow_y*(1-0.75); 00260 }*/ 00261 data.lnStop[Y_LINE]=1; 00262 } 00263 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){ 00264 00265 data.ReturnDir[Y_LINE]=B_LINE; 00266 00267 if(pow_y<0){ 00268 *y = 0; 00269 data.lnStop[Y_LINE]=1; 00270 } 00271 else{ 00272 *y = -LineReturn[4]; 00273 data.lnStop[Y_LINE]=0; 00274 } 00275 } 00276 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){ 00277 00278 data.ReturnDir[Y_LINE]=F_LINE; 00279 00280 if(pow_y>0){ 00281 *y = 0; 00282 data.lnStop[Y_LINE]=1; 00283 } 00284 else{ 00285 *y = LineReturn[4]; 00286 data.lnStop[Y_LINE]=0; 00287 } 00288 } 00289 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){ 00290 if( 00291 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==B_SPOT)|| 00292 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==A_SPOT) 00293 ){ 00294 *y = LineReturn[4]; 00295 data.lnStop[Y_LINE]=0; 00296 } 00297 else if( 00298 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==C_SPOT)|| 00299 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==C_SPOT) 00300 ){ 00301 *y = -LineReturn[4]; 00302 data.lnStop[Y_LINE]=0; 00303 } 00304 else{ 00305 *y = 0; 00306 data.lnStop[Y_LINE]=0; 00307 } 00308 } 00309 } 00310 //none 00311 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){ 00312 //turn! 00313 } 00314 00315 } 00316 else{//data.FieldSpot==LINE_INSIDE 00317 00318 data.ReturnDir[X_LINE]=LINE_EMPTY; 00319 data.ReturnDir[Y_LINE]=LINE_EMPTY; 00320 00321 *x = 0; 00322 *y = 0; 00323 00324 //line間際の復帰力以外の力を作用させるか否か 00325 data.lnStop[X_LINE]=1; 00326 data.lnStop[Y_LINE]=1; 00327 /* 00328 if( 00329 (data.lnRawMemory[A_SPOT]==1)&& 00330 (data.lnRawMemory[B_SPOT]==1)&& 00331 (data.lnRawMemory[C_SPOT]==0) 00332 ){ 00333 data.lnStop[Y_LINE]=0; 00334 *y = -LineReturn[4]; 00335 } 00336 else if( 00337 (data.lnRawMemory[A_SPOT]==0)&& 00338 (data.lnRawMemory[B_SPOT]==0)&& 00339 (data.lnRawMemory[C_SPOT]==1) 00340 ){ 00341 data.lnStop[Y_LINE]=0; 00342 *y = LineReturn[4]; 00343 }*/ 00344 } 00345 } 00346 void LineJudgeReturn2(double pow_x, double pow_y, double *x, double *y){ 00347 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20}; 00348 uint8_t LinePingState[4]; 00349 //JSO2 00350 static uint8_t lnRawReturn1 = 0; 00351 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される． 00352 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される． 00353 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する． 00354 //×ラインのほぼ場外では常時ラインから離れる力が優先される． 00355 //※場外判定を行うには再び場内に戻る必要がある． 00356 00357 data.FieldSpot=LINE_INSIDE; 00358 00359 if(data.FieldSpot==LINE_OUTSIDE){ 00360 } 00361 else{//data.FieldSpot==LINE_INSIDE 00362 00363 data.ReturnDir[X_LINE]=LINE_EMPTY; 00364 data.ReturnDir[Y_LINE]=LINE_EMPTY; 00365 00366 *x = 0; 00367 *y = 0; 00368 00369 //line間際の復帰力以外の力を作用させるか否か 00370 data.lnStop[X_LINE]=1; 00371 data.lnStop[Y_LINE]=1; 00372 00373 if(//y 00374 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==B_SPOT)&&(1))|| 00375 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==A_SPOT)&&(1)) 00376 ){ 00377 data.lnStop[Y_LINE]=0; 00378 *y = -LineReturn[4]; 00379 //JSO1 00380 if( 00381 (data.ping[F_PING]<45)&& 00382 (data.lnRawReturn==1)//JSO2 00383 ){ 00384 data.lnStop[X_LINE]=0; 00385 } 00386 //JSO2 00387 if( 00388 ( 00389 ((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==0))|| 00390 ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)) 00391 )&& 00392 (data.ping[F_PING]>45)&& 00393 (data.ping[F_PING]<200)&& 00394 (data.ping[R_PING]<25)&& 00395 (data.ping[L_PING]>40) 00396 ){ 00397 *x = -LineReturn[4]; 00398 data.lnStop[X_LINE]=0; 00399 } 00400 if( 00401 ( 00402 ((Line[A_SPOT].read()==0)&&(Line[B_SPOT].read()==1))|| 00403 ((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)) 00404 )&& 00405 (data.ping[F_PING]>45)&& 00406 (data.ping[F_PING]<200)&& 00407 (data.ping[L_PING]<25)&& 00408 (data.ping[R_PING]>40) 00409 ){ 00410 *x = LineReturn[4]; 00411 data.lnStop[X_LINE]=0; 00412 } 00413 } 00414 else if( 00415 ((data.lnRawOrder[0]==C_SPOT)&&(1)&&(1)) 00416 ){ 00417 data.lnStop[Y_LINE]=0; 00418 *y = LineReturn[4]; 00419 00420 //JSO1 00421 if( 00422 (data.ping[B_PING]<45)&& 00423 (data.lnRawReturn==1) 00424 ){ 00425 data.lnStop[X_LINE]=0; 00426 } 00427 } 00428 else if(//x 00429 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) 00430 ){ 00431 *x = -LineReturn[4]; 00432 data.lnStop[X_LINE]=0; 00433 } 00434 else if( 00435 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) 00436 ){ 00437 *x = LineReturn[4]; 00438 data.lnStop[X_LINE]=0; 00439 } 00440 00441 if( 00442 ((*x)!=0)|| 00443 ((*y)!=0) 00444 ){ 00445 00446 data.lnRawReturn=1; 00447 00448 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWallPlus[X_PING]); 00449 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWallPlus[X_PING]); 00450 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWallPlus[Y_PING]); 00451 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWallPlus[Y_PING]); 00452 //x 00453 if(data.lnStop[X_LINE]==1){ 00454 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ 00455 *x = -LineReturn[4]; 00456 data.lnStop[X_LINE]=0; 00457 } 00458 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ 00459 *x = LineReturn[4]; 00460 data.lnStop[X_LINE]=0; 00461 } 00462 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){ 00463 *x = 0; 00464 data.lnStop[X_LINE]=0; 00465 } 00466 } 00467 //y 00468 if(data.lnStop[Y_LINE]==1){ 00469 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){ 00470 if(pow_y<0){ 00471 *y = 0; 00472 data.lnStop[Y_LINE]=1; 00473 } 00474 else{ 00475 *y = -LineReturn[4]; 00476 data.lnStop[Y_LINE]=0; 00477 } 00478 } 00479 //JSO2 00480 if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)&&(data.lnRawReturn==1)){ 00481 *y = -LineReturn[4]; 00482 data.lnStop[Y_LINE]=0; 00483 } 00484 00485 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){ 00486 if(pow_y>0){ 00487 *y = 0; 00488 data.lnStop[Y_LINE]=1; 00489 } 00490 else{ 00491 *y = LineReturn[4]; 00492 data.lnStop[Y_LINE]=0; 00493 } 00494 } 00495 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){ 00496 *y = 0; 00497 data.lnStop[Y_LINE]=0; 00498 } 00499 } 00500 } 00501 else{ 00502 data.lnRawReturn=0; 00503 00504 //JSO3 00505 if((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x>0)){ 00506 data.lnStop[X_LINE]=0; 00507 } 00508 if((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x<0)){ 00509 data.lnStop[X_LINE]=0; 00510 } 00511 //JSO3 00512 if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){ 00513 data.lnStop[Y_LINE]=0; 00514 } 00515 } 00516 //JSO3 00517 if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){ 00518 data.lnStop[Y_LINE]=0; 00519 } 00520 //JSO1 00521 if(data.lnRawReturn==1){ 00522 //JSO2 00523 if((lnRawReturn1==0)){ 00524 LineLiberate(); 00525 lnRawReturn1=1; 00526 } 00527 00528 if( 00529 ( 00530 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))|| 00531 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) 00532 )&& 00533 //((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==1))&&//JSO1 00534 ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1))&&//JSO2 00535 ((data.ping[F_PING]<45)&&(1))&&//JSO2 00536 //(!((data.ping[B_PING]<40)&&(data.ping[B_PING]>25)))&&//JSO2 00537 ((data.lnRawOrder[0]!=C_SPOT)&&(data.lnRawOrderLog1[0]!=C_SPOT)&&(data.lnRawOrderLog2[0]!=C_SPOT)) 00538 00539 ){ 00540 *y = -LineReturn[4]; 00541 data.lnStop[Y_LINE]=0;//JSO2 00542 } 00543 } 00544 else{ 00545 lnRawReturn1=0; 00546 00547 00548 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ 00549 *x = -LineReturn[4]; 00550 data.lnStop[X_LINE]=0; 00551 } 00552 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ 00553 *x = LineReturn[4]; 00554 data.lnStop[X_LINE]=0; 00555 } 00556 00557 } 00558 00559 } 00560 } 00561 void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){ 00562 //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; 00563 //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; 00564 static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; 00565 static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; 00566 00567 if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){ 00568 if(data.FieldSpot==LINE_INSIDE){ 00569 00570 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]); 00571 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]); 00572 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]); 00573 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]); 00574 if( 00575 (data.lnCorner[L_LINE])|| 00576 (data.lnCorner[R_LINE])|| 00577 (data.lnCorner[F_LINE])|| 00578 (data.lnCorner[B_LINE]) 00579 ){ 00580 00581 data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]); 00582 data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]); 00583 data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]); 00584 data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]); 00585 00586 00587 00588 ///* 00589 LastLineCorner[L_LINE]=NewLineCorner[L_LINE]; 00590 LastLineCorner[R_LINE]=NewLineCorner[R_LINE]; 00591 LastLineCorner[F_LINE]=NewLineCorner[F_LINE]; 00592 LastLineCorner[B_LINE]=NewLineCorner[B_LINE]; 00593 00594 NewLineCorner[L_LINE]=data.lnCorner[L_LINE]; 00595 NewLineCorner[R_LINE]=data.lnCorner[R_LINE]; 00596 NewLineCorner[F_LINE]=data.lnCorner[F_LINE]; 00597 NewLineCorner[B_LINE]=data.lnCorner[B_LINE]; 00598 00599 if( 00600 (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&& 00601 (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&& 00602 (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&& 00603 (LastLineCorner[B_LINE]==NewLineCorner[B_LINE]) 00604 ){ 00605 data.lnRepeat++; 00606 } 00607 else{ 00608 data.lnRepeat=0; 00609 } 00610 //*/ 00611 00612 data.FieldSpot = LINE_OUTSIDE; 00613 LineLiberate(); 00614 } 00615 } 00616 else if(data.FieldSpot==LINE_OUTSIDE){ 00617 if(data.lnRaw==0){ 00618 data.FieldSpot = LINE_INSIDE; 00619 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; 00620 LineLiberate(); 00621 LineRankClear(); 00622 } 00623 } 00624 } 00625 if(data.FieldSpot == LINE_OUTSIDE){ 00626 if( 00627 ( 00628 (data.ping[L_PING]>=WhiteToWall[X_PING])|| 00629 (data.NonWall[L_LINE]==1)|| 00630 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) 00631 //(data.lnOrder[0]==A_SPOT) 00632 )&& 00633 ( 00634 (data.ping[R_PING]>=WhiteToWall[X_PING])|| 00635 (data.NonWall[R_LINE]==1)|| 00636 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) 00637 //(data.lnOrder[0]==B_SPOT) 00638 )&& 00639 ( 00640 (data.ping[F_PING]>=WhiteToWall[Y_PING])|| 00641 (data.NonWall[F_LINE]==1)|| 00642 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| 00643 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) 00644 )&& 00645 ((data.ping[B_PING]>=WhiteToWall[Y_PING])|| 00646 (data.NonWall[B_LINE]==1)|| 00647 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| 00648 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) 00649 ) 00650 ){ 00651 data.FieldSpot = LINE_INSIDE; 00652 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; 00653 LineLiberate(); 00654 LineRankClear(); 00655 } 00656 } 00657 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){ 00658 if( 00659 ( 00660 ( 00661 (data.ping[L_PING]>=WhiteToWall[X_PING])|| 00662 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) 00663 //(data.lnOrder[0]==A_SPOT) 00664 )&& 00665 ( 00666 (data.ping[R_PING]>=WhiteToWall[X_PING])|| 00667 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) 00668 //(data.lnOrder[0]==B_SPOT) 00669 )&& 00670 ( 00671 (data.ping[F_PING]>=WhiteToWall[Y_PING])|| 00672 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| 00673 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) 00674 )&& 00675 ( 00676 (data.ping[B_PING]>=WhiteToWall[Y_PING])|| 00677 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| 00678 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) 00679 ) 00680 ) 00681 || 00682 ( 00683 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& 00684 (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) 00685 ) 00686 ){ 00687 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; 00688 LineLiberate(); 00689 } 00690 } 00691 ///* 00692 if( 00693 (data.irNotice==IR_NONE)|| 00694 (data.irNotice==IR_FAR)|| 00695 ( 00696 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& 00697 (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) 00698 ) 00699 ){ 00700 data.lnRepeat = 0; 00701 00702 NewLineCorner[L_LINE]=LINE_EMPTY; 00703 NewLineCorner[R_LINE]=LINE_EMPTY; 00704 NewLineCorner[F_LINE]=LINE_EMPTY; 00705 NewLineCorner[B_LINE]=LINE_EMPTY; 00706 00707 LastLineCorner[L_LINE]=LINE_EMPTY; 00708 LastLineCorner[R_LINE]=LINE_EMPTY; 00709 LastLineCorner[F_LINE]=LINE_EMPTY; 00710 LastLineCorner[B_LINE]=LINE_EMPTY; 00711 } 00712 //data.lnRepeat=0; 00713 if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){ 00714 //x 00715 if( 00716 ((pow_x>=0)&&(NewLineCorner[R_LINE]))|| 00717 ((pow_x<0)&&(NewLineCorner[L_LINE])) 00718 ){ 00719 data.lnStay[X_LINE]=0; 00720 } 00721 else{ 00722 data.lnStay[X_LINE]=1; 00723 } 00724 //y 00725 if( 00726 ((pow_y>=0)&&(NewLineCorner[F_LINE]))|| 00727 ((pow_y<0)&&(NewLineCorner[B_LINE])) 00728 ){ 00729 data.lnStay[Y_LINE]=0; 00730 } 00731 else{ 00732 data.lnStay[Y_LINE]=1; 00733 } 00734 } 00735 else{ 00736 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; 00737 } 00738 //*/ 00739 00740 } 00741 void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){ 00742 00743 if(data.lnRepeat==0){ 00744 data.lnStayNow[X_LINE]=0; 00745 data.lnStayNow[Y_LINE]=0; 00746 } 00747 data.lnStay[X_LINE]=1; 00748 data.lnStay[Y_LINE]=1; 00749 00750 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){ 00751 data.lnRepeat=1; 00752 } 00753 00754 if( 00755 (data.irNotice==IR_NONE)|| 00756 //(data.irNotice==IR_FAR)|| 00757 //(data.irLastPosition<=7)|| 00758 ( 00759 (!( 00760 (data.irPosition==data.irLastPosition)|| 00761 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))|| 00762 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))|| 00763 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))|| 00764 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12])) 00765 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))|| 00766 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0)) 00767 ))&&( 00768 (data.irLastNotice==IR_CLOSE)|| 00769 (data.irLastNotice==IR_CLOSER) 00770 ) 00771 ) 00772 ){ 00773 data.lnRepeat = 0; 00774 00775 LineRawLogReset(); 00776 } 00777 //data.lnRepeat=0; 00778 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){ 00779 if( 00780 (data.lnRepeat>=1)&& 00781 (data.lnRawOrder[0]==LINE_EMPTY)/*&& 00782 ( 00783 ( 00784 (data.irPosition==data.irLastPosition)|| 00785 (data.irPosition==(data.irLastPosition+1))|| 00786 (data.irPosition==(data.irLastPosition-1))|| 00787 (data.irPosition==(data.irLastPosition+11))|| 00788 (data.irPosition==(data.irLastPosition-11)) 00789 )&& 00790 ( 00791 (data.irNotice==IR_CLOSE)|| 00792 (data.irNotice==IR_CLOSER) 00793 ) 00794 )*/ 00795 ){ 00796 //y 00797 if( 00798 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))|| 00799 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1))) 00800 ){ 00801 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ 00802 data.lnStay[Y_LINE]=0; 00803 } 00804 else{ 00805 data.lnStay[Y_LINE]=1; 00806 } 00807 data.lnStay[Y_LINE]=0; 00808 data.lnStayNow[Y_LINE]=1; 00809 } 00810 else{ 00811 data.lnStay[Y_LINE]=1; 00812 } 00813 if( 00814 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1))) 00815 ){ 00816 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ 00817 data.lnStay[Y_LINE]=0; 00818 } 00819 else{ 00820 data.lnStay[Y_LINE]=1; 00821 } 00822 data.lnStay[Y_LINE]=0; 00823 data.lnStayNow[Y_LINE]=1; 00824 } 00825 else{ 00826 data.lnStay[Y_LINE]=1; 00827 } 00828 //x 00829 if( 00830 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) 00831 ){ 00832 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ 00833 data.lnStay[X_LINE]=0; 00834 } 00835 else{ 00836 data.lnStay[X_LINE]=1; 00837 } 00838 data.lnStay[X_LINE]=0; 00839 data.lnStayNow[X_LINE]=1; 00840 } 00841 else{ 00842 data.lnStay[X_LINE]=1; 00843 } 00844 if( 00845 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) 00846 ){ 00847 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ 00848 data.lnStay[X_LINE]=0; 00849 } 00850 else{ 00851 data.lnStay[X_LINE]=1; 00852 } 00853 data.lnStay[X_LINE]=0; 00854 data.lnStayNow[X_LINE]=1; 00855 } 00856 else{ 00857 data.lnStay[X_LINE]=1; 00858 } 00859 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){ 00860 //y2 00861 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ 00862 if( 00863 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 00864 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 00865 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19))) 00866 ){ 00867 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ 00868 data.lnStay[X_LINE]=0; 00869 } 00870 else{ 00871 data.lnStay[X_LINE]=1; 00872 } 00873 if(data.lnRepeat>=LINE_REPEAT){ 00874 data.lnStay[X_LINE]=0; 00875 } 00876 data.lnStayNow[X_LINE]=1; 00877 } 00878 else{ 00879 data.lnStay[X_LINE]=1; 00880 } 00881 } 00882 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ 00883 if( 00884 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| 00885 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| 00886 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16))) 00887 ){ 00888 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ 00889 data.lnStay[X_LINE]=0; 00890 } 00891 else{ 00892 data.lnStay[X_LINE]=1; 00893 } 00894 if(data.lnRepeat>=LINE_REPEAT){ 00895 data.lnStay[X_LINE]=0; 00896 } 00897 data.lnStayNow[X_LINE]=1; 00898 } 00899 else{ 00900 data.lnStay[X_LINE]=1; 00901 } 00902 } 00903 //x2 00904 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){ 00905 if( 00906 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 00907 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13))) 00908 ){ 00909 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ 00910 data.lnStay[Y_LINE]=0; 00911 } 00912 else{ 00913 data.lnStay[Y_LINE]=1; 00914 } 00915 if(data.lnRepeat>=LINE_REPEAT){ 00916 data.lnStay[Y_LINE]=0; 00917 } 00918 data.lnStayNow[Y_LINE]=1; 00919 } 00920 else{ 00921 data.lnStay[Y_LINE]=1; 00922 } 00923 00924 if( 00925 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))|| 00926 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16))) 00927 ){ 00928 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ 00929 data.lnStay[Y_LINE]=0; 00930 } 00931 else{ 00932 data.lnStay[Y_LINE]=1; 00933 } 00934 if(data.lnRepeat>=LINE_REPEAT){ 00935 data.lnStay[Y_LINE]=0; 00936 } 00937 data.lnStayNow[Y_LINE]=1; 00938 } 00939 else{ 00940 data.lnStay[Y_LINE]=1; 00941 } 00942 } 00943 if( 00944 (data.lnRepeat>=2) 00945 ){ 00946 if( 00947 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))|| 00948 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1))) 00949 ){ 00950 data.lnStay[Y_LINE]=0; 00951 } 00952 if( 00953 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))|| 00954 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1))) 00955 ){ 00956 data.lnStay[X_LINE]=0; 00957 } 00958 } 00959 } 00960 00961 00962 } 00963 else{ 00964 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; 00965 } 00966 00967 } 00968 void LineJudgeReset3(double pow_x, double pow_y, double *x, double *y){ 00969 00970 if(data.lnRepeat==0){ 00971 data.lnStayNow[X_LINE]=0; 00972 data.lnStayNow[Y_LINE]=0; 00973 } 00974 data.lnStay[X_LINE]=1; 00975 data.lnStay[Y_LINE]=1; 00976 00977 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){ 00978 data.lnRepeat=1; 00979 } 00980 00981 if( 00982 (data.irNotice==IR_NONE)|| 00983 (data.irNotice==IR_FAR)|| 00984 //(data.irLastPosition<=7)|| 00985 ( 00986 (!( 00987 (data.irPosition==data.irLastPosition)|| 00988 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))|| 00989 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12])) 00990 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))|| 00991 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12])) 00992 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))|| 00993 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0)) 00994 ))&&( 00995 (data.irLastNotice==IR_CLOSE)|| 00996 (data.irLastNotice==IR_CLOSER) 00997 ) 00998 ) 00999 ){ 01000 data.lnRepeat = 0; 01001 01002 LineRawLogReset(); 01003 } 01004 //data.lnRepeat=0; 01005 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){ 01006 if( 01007 (data.lnRepeat>=1)&& 01008 (data.lnRawOrder[0]==LINE_EMPTY)/*&& 01009 ( 01010 ( 01011 (data.irPosition==data.irLastPosition)|| 01012 (data.irPosition==(data.irLastPosition+1))|| 01013 (data.irPosition==(data.irLastPosition-1))|| 01014 (data.irPosition==(data.irLastPosition+11))|| 01015 (data.irPosition==(data.irLastPosition-11)) 01016 )&& 01017 ( 01018 (data.irNotice==IR_CLOSE)|| 01019 (data.irNotice==IR_CLOSER) 01020 ) 01021 )*/ 01022 ){ 01023 //y 01024 if( 01025 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))|| 01026 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1))) 01027 ){ 01028 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ 01029 data.lnStay[Y_LINE]=0; 01030 } 01031 else{ 01032 data.lnStay[Y_LINE]=1; 01033 } 01034 data.lnStay[Y_LINE]=0; 01035 data.lnStayNow[Y_LINE]=1; 01036 } 01037 else{ 01038 data.lnStay[Y_LINE]=1; 01039 } 01040 if( 01041 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1))) 01042 ){ 01043 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ 01044 data.lnStay[Y_LINE]=0; 01045 } 01046 else{ 01047 data.lnStay[Y_LINE]=1; 01048 } 01049 data.lnStay[Y_LINE]=0; 01050 data.lnStayNow[Y_LINE]=1; 01051 } 01052 else{ 01053 data.lnStay[Y_LINE]=1; 01054 } 01055 //x 01056 if( 01057 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) 01058 ){ 01059 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ 01060 data.lnStay[X_LINE]=0; 01061 } 01062 else{ 01063 data.lnStay[X_LINE]=1; 01064 } 01065 data.lnStay[X_LINE]=0; 01066 data.lnStayNow[X_LINE]=1; 01067 } 01068 else{ 01069 data.lnStay[X_LINE]=1; 01070 } 01071 if( 01072 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) 01073 ){ 01074 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ 01075 data.lnStay[X_LINE]=0; 01076 } 01077 else{ 01078 data.lnStay[X_LINE]=1; 01079 } 01080 data.lnStay[X_LINE]=0; 01081 data.lnStayNow[X_LINE]=1; 01082 } 01083 else{ 01084 data.lnStay[X_LINE]=1; 01085 } 01086 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){ 01087 //y2 01088 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ 01089 if( 01090 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 01091 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 01092 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19))) 01093 ){ 01094 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ 01095 data.lnStay[X_LINE]=0; 01096 } 01097 else{ 01098 data.lnStay[X_LINE]=1; 01099 } 01100 if(data.lnRepeat>=LINE_REPEAT){ 01101 data.lnStay[X_LINE]=0; 01102 } 01103 data.lnStayNow[X_LINE]=1; 01104 } 01105 else{ 01106 data.lnStay[X_LINE]=1; 01107 } 01108 } 01109 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ 01110 if( 01111 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| 01112 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| 01113 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16))) 01114 ){ 01115 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ 01116 data.lnStay[X_LINE]=0; 01117 } 01118 else{ 01119 data.lnStay[X_LINE]=1; 01120 } 01121 if(data.lnRepeat>=LINE_REPEAT){ 01122 data.lnStay[X_LINE]=0; 01123 } 01124 data.lnStayNow[X_LINE]=1; 01125 } 01126 else{ 01127 data.lnStay[X_LINE]=1; 01128 } 01129 } 01130 //x2 01131 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){ 01132 if( 01133 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| 01134 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13))) 01135 ){ 01136 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ 01137 data.lnStay[Y_LINE]=0; 01138 } 01139 else{ 01140 data.lnStay[Y_LINE]=1; 01141 } 01142 if(data.lnRepeat>=LINE_REPEAT){ 01143 data.lnStay[Y_LINE]=0; 01144 } 01145 data.lnStayNow[Y_LINE]=1; 01146 } 01147 else{ 01148 data.lnStay[Y_LINE]=1; 01149 } 01150 01151 if( 01152 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))|| 01153 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16))) 01154 ){ 01155 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ 01156 data.lnStay[Y_LINE]=0; 01157 } 01158 else{ 01159 data.lnStay[Y_LINE]=1; 01160 } 01161 if(data.lnRepeat>=LINE_REPEAT){ 01162 data.lnStay[Y_LINE]=0; 01163 } 01164 data.lnStayNow[Y_LINE]=1; 01165 } 01166 else{ 01167 data.lnStay[Y_LINE]=1; 01168 } 01169 } 01170 if( 01171 (data.lnRepeat>=2) 01172 ){ 01173 if( 01174 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))|| 01175 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1))) 01176 ){ 01177 data.lnStay[Y_LINE]=0; 01178 } 01179 if( 01180 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))|| 01181 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1))) 01182 ){ 01183 data.lnStay[X_LINE]=0; 01184 } 01185 } 01186 } 01187 01188 01189 } 01190 else{ 01191 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; 01192 } 01193 01194 } 01195 void JudgeInSide(void){ 01196 data.FieldSpot = LINE_INSIDE; 01197 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; 01198 LineLiberate(); 01199 LineRankClear(); 01200 //Line_home.detach(); 01201 }
Generated on Wed Jul 13 2022 02:59:03 by
1.7.2