CatPot 2015-2016
ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Embed:
(wiki syntax)
Show/hide line numbers
strategy.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 //Atk 00005 void modeAttack4(void){ 00006 //irの値に影響するモーター補正値 00007 //uint8_t ir_pow; 00008 uint8_t ir_pow_x, ir_pow_y; 00009 double ir_x_dir, ir_y_dir; 00010 double ir_x_turn, ir_y_turn; 00011 double ir_x, ir_y; 00012 //lineの値に影響するモーター補正値 00013 double LineSlowPower[2]; 00014 double LineReturnPower[2]; 00015 00016 //モーターの出力値 00017 int vx,vy,vs; 00018 00019 ////初期値を決める等 00020 if(sys.KickOffFlag==1){ 00021 /* 00022 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする． 00023 sys.IrBlind=0; 00024 sys.LineBlind=0; 00025 sys.PingBlind=0; 00026 00027 sys.HomeBlind=1; 00028 sys.DriBlind=1; 00029 //defence 00030 sys.DefenceFlag=0; 00031 */ 00032 00033 00034 //Kick 00035 sys.KickStopFlag=0; 00036 //Ir 00037 sys.ir_pow_table = 0; 00038 //Line 00039 //data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY; 00040 data.lnRepeat = 0; 00041 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; 00042 data.lnStayNow[X_LINE]=data.lnStayNow[Y_LINE]=0; 00043 data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1; 00044 00045 //data.FieldSpot = LINE_INSIDE; 00046 //LineLiberate(); 00047 //LineRankClear(); 00048 00049 //JSO2 00050 LineLiberate(); 00051 00052 LineRawRankClear(); 00053 00054 //data.lnRawMemory[A_SPOT]=0; 00055 //data.lnRawMemory[B_SPOT]=0; 00056 //data.lnRawMemory[C_SPOT]=0; 00057 00058 data.lnRawReturn=0; 00059 00060 LineRawLogReset(); 00061 //backhome 00062 sys.HomeStayFlag[X_PING]=0; 00063 sys.HomeStayFlag[Y_PING]=0; 00064 //pid 00065 sys.TurnStopFlag=0; 00066 cmps_set.AtkDeg=0; 00067 cmps_set.HoldDeg=0; 00068 cmps_set.GoalDeg=0; 00069 //ドリブラー 00070 sys.BallHoldJudgeFlag=0; 00071 sys.BallHoldFlag=0; 00072 00073 //初期値設定の終了 00074 sys.KickOffFlag=0; 00075 } 00076 ////DataRetrieve 00077 if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;} 00078 data.lnRaw = LineRaw; 00079 data.lnHold = LineHold; 00080 data.ball = ReadBall(); 00081 00082 //ボールがなければ自分のゴールに戻る 00083 if(data.irNotice==IR_NONE){ 00084 00085 cmps_set.GoalDeg=0; 00086 sys.BackHomeFlag=(sys.HomeBlind==0); 00087 } 00088 else{ 00089 sys.BackHomeFlag=0; 00090 sys.HomeStayFlag[X_PING]=0; 00091 sys.HomeStayFlag[Y_PING]=0; 00092 } 00093 //回り込みの値を代入 00094 if(sys.DefenceFlag==1){ 00095 00096 if((data.irNotice==IR_NONE)||(data.irNotice==IR_FAR)){ 00097 00098 if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)){ 00099 ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR]; 00100 ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN]; 00101 00102 if(data.ping[B_PING]>=10){ 00103 ir_y_dir = -1; 00104 ir_y_turn = 0; 00105 } 00106 else{ 00107 ir_y_dir = 0; 00108 ir_y_turn = 0; 00109 } 00110 } 00111 else if((data.ping[L_PING]>=50)&&(data.ping[R_PING]<50)){ 00112 ir_x_dir = -1; 00113 ir_x_turn = 0; 00114 00115 if(data.ping[B_PING]>=45){ 00116 ir_y_dir = -1; 00117 ir_y_turn = 0; 00118 } 00119 else{ 00120 ir_y_dir = 0; 00121 ir_y_turn = 0; 00122 } 00123 } 00124 else if((data.ping[L_PING]<50)&&(data.ping[R_PING]>=50)){ 00125 ir_x_dir = 1; 00126 ir_x_turn = 0; 00127 00128 if(data.ping[B_PING]>=45){ 00129 ir_y_dir = -1; 00130 ir_y_turn = 0; 00131 } 00132 else{ 00133 ir_y_dir = 0; 00134 ir_y_turn = 0; 00135 } 00136 } 00137 else if((data.ping[L_PING]<50)&&(data.ping[R_PING]<50)){ 00138 ir_x_dir = 0; 00139 ir_x_turn = 0; 00140 00141 if(data.ping[B_PING]>=45){ 00142 ir_y_dir = -1; 00143 ir_y_turn = 0; 00144 } 00145 else{ 00146 ir_y_dir = 0; 00147 ir_y_turn = 0; 00148 } 00149 } 00150 } 00151 else{ 00152 00153 ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR]; 00154 ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN]; 00155 00156 if((data.ping[B_PING]>=5)&&(data.ping[R_PING]>=50)&&(data.ping[L_PING]>=50)){ 00157 ir_y_dir = -1; 00158 ir_y_turn = 0; 00159 } 00160 else{ 00161 ir_y_dir = 0; 00162 ir_y_turn = 0; 00163 } 00164 } 00165 /* 00166 if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)&&(data.ping[B_PING]>10)){ 00167 ir_y_dir = -1; 00168 ir_y_turn = 0; 00169 00170 if(data.ping[B_PING]>=10){ 00171 ir_y_dir = -1; 00172 ir_y_turn = 0; 00173 } 00174 else{ 00175 ir_y_dir = 0; 00176 ir_y_turn = 0; 00177 } 00178 00179 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR]; 00180 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN]; 00181 } 00182 else{ 00183 00184 } 00185 */ 00186 /* 00187 if((data.ping[B_PING]>=45)&&(data.irNotice!=IR_NONE)&&(15<=data.irPosition)&&(data.irPosition<=19)){ 00188 //sys.ir_pow_table=1; 00189 ir_x_dir = 0; 00190 ir_y_dir = -1; 00191 ir_x_turn = 0; 00192 ir_y_turn = 0; 00193 } 00194 else{ 00195 if(data.irValPhase[IR_SHORT]>=DIS_4){ 00196 sys.ir_pow_table=2;//直進 00197 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR]; 00198 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR]; 00199 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN]; 00200 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN]; 00201 } 00202 else{ 00203 if(data.irValPhase[IR_SHORT]<=DIS_1){ 00204 sys.ir_pow_table=0;//3 00205 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; 00206 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; 00207 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; 00208 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; 00209 } 00210 else{ 00211 sys.ir_pow_table=0; 00212 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; 00213 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; 00214 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; 00215 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; 00216 } 00217 } 00218 }*/ 00219 } 00220 else{ 00221 if((data.ping[B_PING]>40)&&(0)){ 00222 sys.ir_pow_table=0; 00223 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; 00224 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; 00225 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; 00226 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; 00227 } 00228 else{ 00229 if((data.ping[B_PING]>60)&&(data.irValPhase[IR_SHORT]>=DIS_4)){ 00230 sys.ir_pow_table=2;//直進 00231 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR]; 00232 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR]; 00233 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN]; 00234 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN]; 00235 } 00236 else{ 00237 if(data.irValPhase[IR_SHORT]<=DIS_1){ 00238 sys.ir_pow_table=0;//3 00239 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; 00240 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; 00241 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; 00242 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; 00243 } 00244 else{ 00245 sys.ir_pow_table=0; 00246 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; 00247 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; 00248 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; 00249 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; 00250 } 00251 } 00252 } 00253 if((sys.HomeBlind==0)){ 00254 if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&& 00255 ( 00256 (data.irPosition==11)|| 00257 (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))|| 00258 (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))|| 00259 (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))|| 00260 (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))|| 00261 (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))|| 00262 (data.irPosition==(ir_posi_s[(11-8+24-3)%12])) 00263 ) 00264 ){ 00265 sys.ir_pow_table=2;//直進 00266 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR]; 00267 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR]; 00268 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN]; 00269 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN]; 00270 } 00271 } 00272 } 00273 00274 //Irの検出値によって出力を調整 00275 if(data.irNotice==IR_CLOSER){ 00276 //ir_pow = sys.s_pow; 00277 ir_pow_x = ir_pow_y = sys.s_pow; 00278 } 00279 else if(data.irNotice==IR_CLOSE){ 00280 //ir_pow = sys.m_pow; 00281 ir_pow_x = ir_pow_y = sys.m_pow; 00282 } 00283 else if(data.irNotice==IR_FAR){ 00284 //ir_pow = sys.l_pow; 00285 ir_pow_x = ir_pow_y = sys.l_pow; 00286 } 00287 else{//data.irNotice==IR_NONE 00288 //ir_pow = 0; 00289 ir_pow_x = ir_pow_y = 0; 00290 } 00291 00292 if(sys.DefenceFlag==1){ 00293 ir_pow_x = 30; 00294 ir_pow_y = 30; 00295 if(data.ping[L_PING]<=60){ 00296 ir_pow_y = 30; 00297 } 00298 if(data.ping[L_PING]<=40){ 00299 ir_pow_y = 25; 00300 } 00301 if(data.ping[L_PING]<=30){ 00302 ir_pow_y = 20; 00303 } 00304 if(data.ping[L_PING]<15){ 00305 ir_pow_y = 15; 00306 } 00307 if(data.ping[L_PING]<10){ 00308 ir_pow_y = 10; 00309 } 00310 } 00311 ////ドリブラーdribbler 00312 //ホールド判定hold 00313 JudgeBallHolding(); 00314 //JudgeBallHold(); 00315 //sys.BallHoldFlag = data.ball; 00316 //ドリブラー駆動 00317 /*if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){ 00318 if(sys.TurnDriBlind==0){ 00319 if( 00320 ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))|| 00321 ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)) 00322 ){ 00323 cmps_set.GoalDeg=0; 00324 } 00325 else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){ 00326 cmps_set.GoalDeg=-30; 00327 } 00328 else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){ 00329 cmps_set.GoalDeg=30; 00330 } 00331 else{ 00332 cmps_set.GoalDeg=0; 00333 } 00334 } 00335 }*/ 00336 if((sys.TurnDriBlind==0)&&(/*data.lnRaw==0*/1)&&((data.irNotice==IR_CLOSER)||(data.irNotice==IR_CLOSE))&&(/*data.ping[B_PING]>90*/1)&& 00337 ( 00338 (data.irPosition==11)|| 00339 (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))|| 00340 (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))|| 00341 (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))|| 00342 (data.irPosition==(ir_posi_s[(11-8+24-2)%12])) 00343 ) 00344 00345 ){ 00346 if(sys.TurnDriBlind==0){ 00347 if( 00348 ((data.ping[L_PING]<40)&&(data.ping[R_PING]>70)) 00349 ){ 00350 cmps_set.GoalDeg=60; 00351 } 00352 else if( 00353 ((data.ping[R_PING]<40)&&(data.ping[L_PING]>70)) 00354 ){ 00355 cmps_set.GoalDeg=-60; 00356 } 00357 else{ 00358 cmps_set.GoalDeg=0; 00359 } 00360 } 00361 } 00362 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){ 00363 sys.DribbleFlag=1; 00364 if((sys.BallHoldFlag==1)||(data.ball==1)){ 00365 //ir_pow=20; 00366 ir_pow_x = ir_pow_y = 20; 00367 } 00368 } 00369 else{ 00370 sys.DribbleFlag=0; 00371 cmps_set.HoldDeg=0; 00372 } 00373 00374 ////Kick 00375 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){ 00376 DriveTurn(); 00377 }*/ 00378 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){ 00379 DriveTurn(); 00380 DriveSolenoid(); 00381 }*/ 00382 if( 00383 /*(sys.BallHoldFlag==1)&& 00384 (data.ball==1)&& 00385 (data.irNotice==IR_CLOSER)&& 00386 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&& 00387 (data.ping[B_PING]>100)&& 00388 ( 00389 ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))|| 00390 ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))|| 00391 (data.lnRawOrderLog1[0]==A_SPOT)|| 00392 (data.lnRawOrder[0]==A_SPOT)|| 00393 (data.lnRawOrderLog1[0]==B_SPOT)|| 00394 (data.lnRawOrder[0]==B_SPOT) 00395 )&& 00396 (data.lnRaw==0)*/ 00397 (sys.DriBlind==0)&& 00398 (data.ping[R_PING]<30)&& 00399 (data.ping[L_PING]<30) 00400 ){ 00401 //DriveTurn(); 00402 //DriveSolenoid(); 00403 } 00404 00405 //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる． 00406 //if(sys.IrBlind==1) ir_pow=0; 00407 if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0; 00408 if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)&&(data.irNotice==IR_CLOSER)){ 00409 ir_x_turn = -ir_x_turn; 00410 ir_y_turn = -ir_y_turn; 00411 } 00412 00413 if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){ 00414 ir_pow_x = ir_pow_y = 25; 00415 //x 00416 if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){ 00417 if(data.ping[L_PING]>data.ping[R_PING]){ 00418 ir_x = -1; 00419 } 00420 else{ 00421 ir_x = 1; 00422 } 00423 if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){ 00424 ir_x = 0; 00425 } 00426 } 00427 else{ 00428 ir_x = 0; 00429 // 00430 sys.HomeStayFlag[X_PING]=1; 00431 } 00432 //y 00433 if((data.ping[B_PING]>60)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){ 00434 ir_y = -1; 00435 /*if((abs(data.ping[L_PING]-data.ping[R_PING])>30)&&(1)){ 00436 ir_pow_y = 15; 00437 }*/ 00438 } 00439 else{ 00440 ir_y = 0; 00441 /* 00442 if((1)&&(data.ping[B_PING]>60)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){ 00443 ir_y = -1; 00444 ir_pow_y = 15; 00445 } 00446 else{ 00447 ir_y = 0; 00448 } 00449 */ 00450 // 00451 sys.HomeStayFlag[Y_PING]=1; 00452 } 00453 } 00454 else{ 00455 ir_x = (ir_x_dir + ir_x_turn); 00456 ir_y = (ir_y_dir + ir_y_turn); 00457 } 00458 if((sys.HomeBlind==0)&&(data.irNotice==IR_FAR)){ 00459 if(ir_y>0){ 00460 ir_y=0; 00461 ir_pow_x = ir_pow_y = 25; 00462 00463 if((data.ping[B_PING]>50)&&(1)){ 00464 ir_y = -1; 00465 //ir_pow_y = 15; 00466 } 00467 00468 } 00469 //ir_pow_x = ir_pow_y = 25; 00470 //if(ir_y>0) ir_y=0; 00471 /*if((data.ping[B_PING]>40)&&(1)){ 00472 ir_y = -1; 00473 //ir_pow_y = 15; 00474 }*/ 00475 } 00476 if( 00477 (sys.DefenceFlag==1)&& 00478 (ir_y>0)&& 00479 (data.ping[B_PING]>60)&& 00480 (data.ping[B_PING]<225)&& 00481 (sys.BackHomeFlag==0) 00482 ){ 00483 ir_pow_y=0; 00484 } 00485 ///dribbler 00486 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&(/*sys.BallHoldFlag==1*/data.ball==1)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){ 00487 00488 if((1/*data.ping[L_PING]<60*/)&&(data.ping[R_PING]>90)){ 00489 //ir_x = 1; 00490 } 00491 else{ 00492 if(data.lnRawOrderLog1[0]==B_SPOT){ 00493 //ir_x = 1; 00494 } 00495 } 00496 if((1/*data.ping[R_PING]<60*/)&&(data.ping[L_PING]>90)){ 00497 //ir_x = -1; 00498 } 00499 else{ 00500 if(data.lnRawOrderLog1[0]==A_SPOT){ 00501 //ir_x = -1; 00502 } 00503 } 00504 if((data.ping[F_PING]<50)&&(data.ping[B_PING]>100)){ 00505 //ir_y = -1; 00506 } 00507 if( 00508 ((data.ping[L_PING]<55)&&(data.ping[R_PING]<55))|| 00509 ((data.ping[L_PING]>55)&&(data.ping[R_PING]>55)) 00510 ){ 00511 //DriveSolenoid(); 00512 } 00513 //x 00514 if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==A_SPOT)){ 00515 ir_x = -1; 00516 } 00517 else if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==B_SPOT)){ 00518 ir_x = 1; 00519 } 00520 else if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){ 00521 if(data.ping[L_PING]>data.ping[R_PING]){ 00522 ir_x = -1; 00523 } 00524 else{ 00525 ir_x = 1; 00526 } 00527 } 00528 else{ 00529 if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&(1)){ 00530 if(data.ping[L_PING]>data.ping[R_PING]){ 00531 ir_x = -1; 00532 } 00533 else{ 00534 ir_x = 1; 00535 } 00536 } 00537 else{ 00538 ir_x = 0; 00539 } 00540 00541 //ir_x = 0; 00542 DriveSolenoid(); 00543 } 00544 //y 00545 if((data.ping[F_PING]<45)&&(abs(data.ping[L_PING]-data.ping[R_PING])>50)){ 00546 //ir_y = 0.75; 00547 } 00548 //kick 00549 if((abs(data.ping[L_PING]-data.ping[R_PING])<30)&&(1)){ 00550 DriveSolenoid(); 00551 } 00552 } 00553 ////Lineセンサーの値を評価しモーターの出力補正値を演算 00554 if(sys.LineBlind==1){ 00555 00556 LineSlowPower[X_LINE] = 1.0; 00557 LineSlowPower[Y_LINE] = 1.0; 00558 00559 LineReturnPower[X_LINE] = 0.0; 00560 LineReturnPower[Y_LINE] = 0.0; 00561 00562 data.lnStop[X_LINE] = 1; 00563 data.lnStop[Y_LINE] = 1; 00564 00565 data.FieldSpot = LINE_INSIDE; 00566 } 00567 else{ 00568 /* 00569 LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]); 00570 LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]); 00571 LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]); 00572 */ 00573 LineJudgeReset3(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]); 00574 LineJudgeSlow2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE], &ir_pow_x, &ir_pow_y); 00575 LineJudgeReturn2(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]); 00576 } 00577 ////LEDデバッグ 00578 //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9; 00579 //if(data.FieldSpot==LINE_INSIDE) LED = 0x6; 00580 //LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0); 00581 //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0); 00582 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0); 00583 00584 //LED = sys.BallHoldFlag*15; 00585 if(sys.BallHoldFlag==0) LED=15; 00586 else LED=10; 00587 /* 00588 if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){ 00589 if(data.ping[L_PING]>data.ping[R_PING]){ 00590 LED=9; 00591 } 00592 else{ 00593 LED=6; 00594 } 00595 } 00596 else{ 00597 LED=15; 00598 }*/ 00599 00600 //if(data.ball==1) LED=15; 00601 //else LED=10; 00602 00603 //if(data.lnRepeat>=1) LED=15; 00604 //else LED=10; 00605 00606 //LED = 0xFF*(data.ping[B_PING]<30); 00607 00608 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0); 00609 00610 //else LED = 0xA; 00611 //LED = LineHold; 00612 00613 00614 ////最終的なモーターの出力を演算 00615 vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE]; 00616 vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE]; 00617 vs = cmps_set.OutputPID; 00618 move( 00619 vx, 00620 vy, 00621 vs 00622 ); 00623 //モーターに信号を出力 00624 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;} 00625 00626 if(sys.stopflag==1){ 00627 //コマンドモードに戻る際の処理 00628 } 00629 return; 00630 } 00631 00632 void modeAttack5(void){ 00633 ////初期値を決める等 00634 if(sys.KickOffFlag==1){ 00635 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする． 00636 sys.IrBlind=0; 00637 sys.LineBlind=0; 00638 sys.PingBlind=0; 00639 00640 sys.HomeBlind=1; 00641 sys.DriBlind=0; 00642 sys.DriMotorBlind=0; 00643 sys.TurnAtkBlind=1; 00644 sys.TurnDriBlind=1; 00645 sys.TurnHoldBlind=1; 00646 sys.KickBlind=0; 00647 //defence 00648 sys.DefenceFlag=0; 00649 //初期値設定の終了 00650 //sys.KickOffFlag=0; 00651 } 00652 modeAttack4(); 00653 }
Generated on Wed Jul 13 2022 02:59:03 by
1.7.2