Nagano kosen robocon
Embed:
(wiki syntax)
Show/hide line numbers
Process.cpp
00001 #include "mbed.h" 00002 #include "Process.h" 00003 #include "QEI.h" 00004 #include <math.h> 00005 00006 #include "../../CommonLibraries/PID/PID.h" 00007 #include "../../CommonLibraries/Ping/Ping.h" 00008 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" 00009 #include "../../Communication/RS485/Master/Master.h" 00010 #include "../../Communication/Controller/Controller.h" 00011 #include "../../Input/ColorSensor/ColorSensor.h" 00012 #include "../../Input/Switch/Switch.h" 00013 #include "../../LED/LED.h" 00014 #include "../../Safty/Safty.h" 00015 #include "../Using.h" 00016 00017 00018 using namespace SWITCH; 00019 using namespace PID_SPACE; 00020 using namespace COLORSENSOR; 00021 00022 static CONTROLLER::ControllerData *controller; 00023 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00024 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; 00025 00026 static bool lock; 00027 static bool processChangeComp; 00028 static int current; 00029 00030 static void AllActuatorReset(); 00031 00032 #ifdef USE_SUBPROCESS 00033 static void (*Process[USE_PROCESS_NUM])(void); 00034 #endif 00035 00036 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE 00037 00038 /*Replace here with the definition code of your variables.*/ 00039 //************足回り***************** 00040 void AllBrake(); 00041 void GoOshituke(); 00042 void GoMae(); 00043 void GoFront(); 00044 void GoBack(); 00045 void GoLeft(); 00046 void GoRight(); 00047 void GoHidariNanameMae(); 00048 void GoHidariNanameUshiro(); 00049 void GoMigiNanameMae(); 00050 void GoMigiNanameUshiro(); 00051 00052 void RightTurning(); 00053 void LeftTurning(); 00054 uint8_t SetStatus(int); 00055 uint8_t SetPWM(int); 00056 bool move_flag = false; 00057 //************足回り***************** 00058 00059 //************カラーセンサ******************** 00060 00061 int Color_FR[3]; //[赤,緑,青] 00062 int Color_FL[3]; 00063 int Color_RL[3]; 00064 int Color_RR[3]; 00065 int intergration = 40; 00066 00067 int Avecolor_FR[3]; 00068 int Avecolor_FL[3]; 00069 int Avecolor_RL[3]; 00070 int Avecolor_RR[3]; 00071 00072 void ColorIn(); 00073 void ColorDetection(); 00074 void getcolor(); 00075 //Ticker catch_colordata; 00076 //************カラーセンサ******************** 00077 00078 //************ライントレース変数******************* 00079 00080 int line_FR[3] = {500, 1000, 1400}; 00081 int line_FL[3] = {400, 1000, 1400}; 00082 int line_RL[3] = {180, 260, 150}; 00083 int line_RR[3] = {300, 1400, 1600}; 00084 /* 00085 int line_FR[3] = {500, 1100, 1500}; 00086 int line_FL[3] = {300, 500, 380}; 00087 int line_RL[3] = {50, 100, 70}; 00088 int line_RR[3] = {300, 500, 300}; 00089 */ 00090 bool white_FR = false; 00091 bool white_FL = false; 00092 bool white_RL = false; 00093 bool white_RR = false; 00094 00095 bool flag = false; 00096 bool invation_FR = false; 00097 bool invation_FL = false; 00098 bool invation_RL = false; 00099 bool invation_RR = false; 00100 bool on_line_flag = false; 00101 00102 void Move_On_the_Line(); 00103 void Move_On_the_Line_Trace(); 00104 void Move_On_the_Line_BackTrace(); 00105 void Move_On_the_Line_BACK(); 00106 void ColorDetection(); 00107 void Color_Changeflag(); 00108 //************ライントレース変数******************* 00109 00110 //************ROタコン****************** 00111 //QEI RtX(Rota_XA_PIN, Rota_XB_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00112 //QEI RtY(Rota_YA_PIN, Rota_YB_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00113 QEI Rota_FR(Rota_FR_A_PIN, Rota_FR_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00114 QEI Rota_FL(Rota_FL_A_PIN, Rota_FL_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00115 QEI Rota_RL(Rota_RL_A_PIN, Rota_RL_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00116 QEI Rota_RR(Rota_RR_A_PIN, Rota_RR_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); 00117 00118 Ticker ugokunoka_rps; 00119 Ticker catch_rps; 00120 //PID Rota_X = PID(0.03, -100, 50, 0.05, 0, 0); 00121 //PID Rota_Y = PID(0.03, -100, 50, 0.05, 0, 0); 00122 PID Get_pwm_FR = PID(0.03, -80, 80, 0.3,0,0); 00123 PID Get_pwm_FL = PID(0.03, -80, 80, 0.3, 0, 0); 00124 PID Get_pwm_RL = PID(0.03, -80, 80, 0.3, 0, 0); 00125 PID Get_pwm_RR = PID(0.03, -80, 80, 0.3, 0, 0); 00126 00127 PID Catch_pwm_FR = PID(0.03, -100, 100, 0.3,0,0); 00128 PID Catch_pwm_FL = PID(0.03, -100, 100, 0.3, 0, 0); 00129 PID Catch_pwm_RL = PID(0.03, -100, 100, 0.3, 0, 0); 00130 PID Catch_pwm_RR = PID(0.03, -100, 100, 0.3, 0, 0); 00131 00132 typedef struct { 00133 double FR; 00134 double FL; 00135 double RL; 00136 double RR; 00137 }Tire; 00138 00139 Tire rps; 00140 Tire p_rps; 00141 Tire rotate; 00142 Tire now; 00143 Tire dist; 00144 00145 typedef struct { 00146 double x; 00147 double y; 00148 }Vector; 00149 //double target = 2561.00; 00150 double target_FR = 0.00; 00151 double target_FL = 0.00; 00152 double target_RL = 0.00; 00153 double target_RR = 0.00; 00154 00155 Vector goal = {1000.00,1000.00}; 00156 Vector rpm; 00157 Vector dis; 00158 00159 int palseX; 00160 int palseY; 00161 00162 double wantVal_FL = 0; 00163 double wantVal_FR = 0; 00164 double wantVal_RL = 0; 00165 double wantVal_RR = 0; 00166 00167 typedef struct { 00168 int FR; 00169 int FL; 00170 int RL; 00171 int RR; 00172 }Pwm; 00173 00174 int pwm_FR; 00175 int pwm_FL; 00176 int pwm_RL; 00177 int pwm_RR; 00178 00179 Pwm pwm; 00180 int hontoka = 0; 00181 00182 int vx; 00183 int vy; 00184 double max; 00185 double ratio; 00186 double max_rotate = 4.5; 00187 00188 void Rota_SetStatus(char,double); 00189 void Get_rps(); 00190 void Get_Move(); 00191 void GoBack_Rota(); 00192 //************Buzzer************* 00193 DigitalOut buzzer(BUZZER_PIN); 00194 Ticker BuzzerTimer; 00195 void BuzzerTimer_func(); 00196 00197 //************動き*********** 00198 bool spot2400_flag = false; 00199 bool spot1200_flag = false; 00200 bool spot1500_flag = false; 00201 bool spot1800_flag = false; 00202 bool next_flag = true; 00203 bool bluezone = true; 00204 bool redzone = false; 00205 bool once_flag = true; 00206 bool debug_flag1 = true; 00207 00208 void CheckPoint(); 00209 void MovePoint(); 00210 void GoShoot(); 00211 /******超音波*****/ 00212 Ping Ping_0(PC_2); 00213 Ping Ping_1(PC_3); 00214 Ping Ping_2(PC_0); 00215 Ping Ping_3(PC_1); 00216 int select_kyori=0; 00217 int select = 0; 00218 int distance0 = 0; 00219 int distance1 = 0; 00220 int distance2 = 0; 00221 int distance3 = 0; 00222 bool ugoite_flag = true; 00223 bool debug_flag = true; 00224 bool temae_flag = false; 00225 bool ugokanaide_flag = false; 00226 bool ok_flag = false; 00227 int zyouge = 0; 00228 Serial pc(USBTX, USBRX); 00229 bool ushiroate_flag = false; 00230 bool kaeru_flag = false; 00231 bool twin_flag = false; 00232 bool idoutable_flag = false; 00233 bool itido_flag = true; 00234 00235 int maeR = 0; 00236 int maeL = 0; 00237 int ushiroL = 0; 00238 int ushiroR = 0; 00239 00240 Timeout launchTimer; 00241 void launchTimer_func(); 00242 void launchTimer_func1(); 00243 void launchTimer_func2(); 00244 bool reset_flag = false; 00245 bool rotarota_flag = false; 00246 bool roller_flag = true; 00247 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00248 00249 #ifdef USE_SUBPROCESS 00250 #if USE_PROCESS_NUM>0 00251 static void Process0(void); 00252 #endif 00253 #if USE_PROCESS_NUM>1 00254 static void Process1(void); 00255 #endif 00256 #if USE_PROCESS_NUM>2 00257 static void Process2(void); 00258 #endif 00259 #if USE_PROCESS_NUM>3 00260 static void Process3(void); 00261 #endif 00262 #if USE_PROCESS_NUM>4 00263 static void Process4(void); 00264 #endif 00265 #if USE_PROCESS_NUM>5 00266 static void Process5(void); 00267 #endif 00268 #if USE_PROCESS_NUM>6 00269 static void Process6(void); 00270 #endif 00271 #if USE_PROCESS_NUM>7 00272 static void Process7(void); 00273 #endif 00274 #if USE_PROCESS_NUM>8 00275 static void Process8(void); 00276 #endif 00277 #if USE_PROCESS_NUM>9 00278 static void Process9(void); 00279 #endif 00280 #endif 00281 00282 void SystemProcessInitialize() 00283 { 00284 #pragma region USER-DEFINED_VARIABLE_INIT 00285 /*Replace here with the initialization code of your variables.*/ 00286 catch_rps.attach_us(&Get_rps,500); 00287 //catch_colordata.attach_us(&ColorDetection,30); 00288 //ugokunoka_rps.attach(&Get_Move,0.1); 00289 00290 #pragma endregion USER-DEFINED_VARIABLE_INIT 00291 00292 AllActuatorReset(); 00293 00294 lock = true; 00295 processChangeComp = true; 00296 current = DEFAULT_PROCESS; 00297 00298 #ifdef USE_SUBPROCESS 00299 #if USE_PROCESS_NUM>0 00300 Process[0] = Process0; 00301 #endif 00302 #if USE_PROCESS_NUM>1 00303 Process[1] = Process1; 00304 #endif 00305 #if USE_PROCESS_NUM>2 00306 Process[2] = Process2; 00307 #endif 00308 #if USE_PROCESS_NUM>3 00309 Process[3] = Process3; 00310 #endif 00311 #if USE_PROCESS_NUM>4 00312 Process[4] = Process4; 00313 #endif 00314 #if USE_PROCESS_NUM>5 00315 Process[5] = Process5; 00316 #endif 00317 #if USE_PROCESS_NUM>6 00318 Process[6] = Process6; 00319 #endif 00320 #if USE_PROCESS_NUM>7 00321 Process[7] = Process7; 00322 #endif 00323 #if USE_PROCESS_NUM>8 00324 Process[8] = Process8; 00325 #endif 00326 #if USE_PROCESS_NUM>9 00327 Process[9] = Process9; 00328 #endif 00329 #endif 00330 } 00331 00332 static void SystemProcessUpdate() 00333 { 00334 #ifdef USE_SUBPROCESS 00335 if(controller->Button.HOME) lock = false; 00336 00337 if(controller->Button.START && processChangeComp) { 00338 current++; 00339 if (USE_PROCESS_NUM-1 < current) current = 0; 00340 processChangeComp = false; 00341 } else if(controller->Button.SELECT && processChangeComp) { 00342 current--; 00343 if (current < 0) current = USE_PROCESS_NUM-1; 00344 processChangeComp = false; 00345 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00346 #endif 00347 00348 #ifdef USE_MOTOR 00349 ACTUATORHUB::MOTOR::Motor::Update(motor); 00350 #endif 00351 00352 #ifdef USE_SOLENOID 00353 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00354 #endif 00355 00356 #ifdef USE_RS485 00357 MASTER::Master::Update(); 00358 ACTUATORHUB::ActuatorHub::Update(); 00359 #endif 00360 00361 } 00362 00363 void SystemProcess() 00364 { 00365 SystemProcessInitialize(); 00366 00367 while(1) { 00368 #ifdef USE_MU 00369 controller = CONTROLLER::Controller::GetData(); 00370 #endif 00371 00372 #ifdef USE_ERRORCHECK 00373 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { 00374 CONTROLLER::Controller::DataReset(); 00375 //AllActuatorReset(); 00376 //lock = true; 00377 } else 00378 #endif 00379 { 00380 #ifdef USE_SUBPROCESS 00381 if(!lock) { 00382 Process[current](); 00383 } else 00384 #endif 00385 { 00386 //ロック時の処理 00387 } 00388 } 00389 LED_DEBUG0 = masterReceive.ems ? LED_ON : LED_OFF; 00390 if(masterReceive.zoneIsRed) 00391 { 00392 redzone = true; 00393 bluezone = false; 00394 }else if(!masterReceive.zoneIsRed) 00395 { 00396 redzone = false; 00397 bluezone = true; 00398 } 00399 if(masterReceive.ems) 00400 { 00401 masterSend.lowerRoller = false; 00402 masterSend.higherRoller = false; 00403 masterSend.launch = false; 00404 move_flag = false; 00405 white_FR = false; 00406 white_FL = false; 00407 white_RL = false; 00408 white_RR = false; 00409 flag = false; 00410 invation_FR = false; 00411 invation_FL = false; 00412 invation_RL = false; 00413 invation_RR = false; 00414 on_line_flag = false; 00415 spot2400_flag = false; 00416 spot1200_flag = false; 00417 spot1500_flag = false; 00418 spot1800_flag = false; 00419 next_flag = true; 00420 once_flag = true; 00421 debug_flag1 = true; 00422 ugoite_flag = true; 00423 debug_flag = true; 00424 temae_flag = false; 00425 ugokanaide_flag = false; 00426 ok_flag = false; 00427 zyouge = 0; 00428 ushiroate_flag = false; 00429 kaeru_flag = false; 00430 twin_flag = false; 00431 idoutable_flag = false; 00432 itido_flag = true; 00433 roller_flag = true; 00434 reset_flag = false; 00435 current = 0; 00436 AllBrake(); 00437 Rota_FR.reset(); 00438 Rota_FL.reset(); 00439 Rota_RL.reset(); 00440 Rota_RR.reset(); 00441 rotarota_flag = false; 00442 masterSend.rollerHeight = STOP; 00443 lock = false; 00444 } 00445 masterSend.processNum = lock ? 0x0f : current; 00446 00447 static uint16_t cnt = 0; 00448 if(cnt >= 0x3fff) { 00449 LED_DEBUG0 = !LED_DEBUG0; 00450 cnt = 0; 00451 } else cnt++; 00452 00453 SystemProcessUpdate(); 00454 } 00455 } 00456 00457 #pragma region PROCESS 00458 #ifdef USE_SUBPROCESS 00459 #if USE_PROCESS_NUM>0 00460 static void Process0()//二段テーブルへ移動か 00461 { 00462 /* 00463 motor[TIRE_FR].dir = FOR; 00464 motor[TIRE_FL].dir = BACK; 00465 motor[TIRE_RL].dir = BACK; 00466 motor[TIRE_RR].dir = FOR; 00467 motor[TIRE_FR].pwm = 49+maeR; 00468 motor[TIRE_FL].pwm = 52+maeL; 00469 motor[TIRE_RL].pwm = 55+ushiroL; 00470 motor[TIRE_RR].pwm = 57+ushiroR; 00471 if(controller->Button.R) 00472 { 00473 maeR++; 00474 }else if(controller->Button.L) 00475 { 00476 maeL++; 00477 }else if(controller->Button.ZL) 00478 { 00479 ushiroL++; 00480 }else if(controller->Button.ZR) 00481 { 00482 ushiroR++; 00483 }else if(controller->Button.A) 00484 { 00485 maeR = 0; 00486 maeL = 0; 00487 ushiroL = 0; 00488 ushiroR = 0; 00489 }*/ 00490 /* 00491 if(controller->Button.UP) 00492 { 00493 GoMae(); 00494 }else if(controller->Button.LEFT) 00495 { 00496 GoHidariNanameMae(); 00497 }else if(controller->Button.RIGHT) 00498 { 00499 GoMigiNanameMae(); 00500 }else{ 00501 AllBrake(); 00502 } 00503 */ 00504 00505 //ColorDetection(); 00506 Color_Changeflag(); 00507 if(bluezone) 00508 { 00509 if(masterReceive.startSw) 00510 { 00511 if(!(GO_2400up||GO_2400down) && GO_1800) 00512 { 00513 move_flag = true; 00514 Rota_FR.reset(); 00515 Rota_FL.reset(); 00516 Rota_RL.reset(); 00517 Rota_RR.reset(); 00518 target_FR = 3500; 00519 target_FL = 3300; 00520 target_RL = -3500; 00521 target_RR = -3300; 00522 masterSend.lowerRoller = true; 00523 masterSend.rollerHeight = UE; 00524 zyouge = 4; 00525 idoutable_flag = true; 00526 }else if(!(GO_2400up||GO_2400down) && GO_1500) 00527 { 00528 move_flag = true; 00529 Rota_FR.reset(); 00530 Rota_FL.reset(); 00531 Rota_RL.reset(); 00532 Rota_RR.reset(); 00533 target_FR = 3300; 00534 target_FL = 2900; 00535 target_RL = -3300; 00536 target_RR = -2900; 00537 masterSend.lowerRoller = true; 00538 masterSend.rollerHeight = MANNAKA; 00539 zyouge = 3; 00540 idoutable_flag = true; 00541 }else if(!(GO_2400up||GO_2400down) && GO_1200) 00542 { 00543 move_flag = true; 00544 Rota_FR.reset(); 00545 Rota_FL.reset(); 00546 Rota_RL.reset(); 00547 Rota_RR.reset(); 00548 target_FR = 2000; 00549 target_FL = 1900; 00550 target_RL = -2000; 00551 target_RR = -1900; 00552 masterSend.lowerRoller = true; 00553 masterSend.rollerHeight = SHITA; 00554 zyouge = 2; 00555 idoutable_flag = true; 00556 }else if(GO_2400up && !GO_2400down) 00557 { 00558 move_flag = true; 00559 Rota_FR.reset(); 00560 Rota_FL.reset(); 00561 Rota_RL.reset(); 00562 Rota_RR.reset(); 00563 target_FR = 300; 00564 target_FL = 2000; 00565 target_RL = -300; 00566 target_RR = -2000; 00567 masterSend.lowerRoller = false; 00568 masterSend.higherRoller = true; 00569 masterSend.rollerHeight = SHITA; 00570 zyouge = 2; 00571 twin_flag = true; 00572 }else if(!GO_2400up && GO_2400down) 00573 { 00574 move_flag = true; 00575 Rota_FR.reset(); 00576 Rota_FL.reset(); 00577 Rota_RL.reset(); 00578 Rota_RR.reset(); 00579 target_FR = 300; 00580 target_FL = 2000; 00581 target_RL = -300; 00582 target_RR = -2000; 00583 masterSend.lowerRoller = true; 00584 masterSend.higherRoller = false; 00585 masterSend.rollerHeight = SHITA; 00586 zyouge = 2; 00587 twin_flag = true; 00588 }else if(GO_2400up && GO_2400down) 00589 { 00590 move_flag = true; 00591 Rota_FR.reset(); 00592 Rota_FL.reset(); 00593 Rota_RL.reset(); 00594 Rota_RR.reset(); 00595 target_FR = 300; 00596 target_FL = 2000; 00597 target_RL = -300; 00598 target_RR = -2000; 00599 masterSend.lowerRoller = true; 00600 masterSend.higherRoller = true; 00601 masterSend.rollerHeight = SHITA; 00602 zyouge = 2; 00603 twin_flag = true; 00604 } 00605 } 00606 }else if(redzone) 00607 { 00608 if(masterReceive.startSw) 00609 { 00610 if(!(GO_2400up||GO_2400down) && GO_1800) 00611 { 00612 move_flag = true; 00613 Rota_FR.reset(); 00614 Rota_FL.reset(); 00615 Rota_RL.reset(); 00616 Rota_RR.reset(); 00617 target_FR = -3300; 00618 target_FL = -3500; 00619 target_RL = 3300; 00620 target_RR = 3500; 00621 masterSend.lowerRoller = true; 00622 masterSend.rollerHeight = UE; 00623 zyouge = 4; 00624 idoutable_flag = true; 00625 }else if(!(GO_2400up||GO_2400down) && GO_1500) 00626 { 00627 move_flag = true; 00628 Rota_FR.reset(); 00629 Rota_FL.reset(); 00630 Rota_RL.reset(); 00631 Rota_RR.reset(); 00632 target_FR = -2900; 00633 target_FL = -3300; 00634 target_RL = 2900; 00635 target_RR = 3300; 00636 masterSend.lowerRoller = true; 00637 masterSend.rollerHeight = MANNAKA; 00638 zyouge = 3; 00639 idoutable_flag = true; 00640 }else if(!(GO_2400up||GO_2400down) && GO_1200) 00641 { 00642 move_flag = true; 00643 Rota_FR.reset(); 00644 Rota_FL.reset(); 00645 Rota_RL.reset(); 00646 Rota_RR.reset(); 00647 target_FR = -1900; 00648 target_FL = -2000; 00649 target_RL = 1900; 00650 target_RR = 2000; 00651 masterSend.lowerRoller = true; 00652 masterSend.rollerHeight = SHITA; 00653 zyouge = 2; 00654 idoutable_flag = true; 00655 }else if(!GO_2400up && GO_2400down) 00656 { 00657 move_flag = true; 00658 Rota_FR.reset(); 00659 Rota_FL.reset(); 00660 Rota_RL.reset(); 00661 Rota_RR.reset(); 00662 target_FR = -2000; 00663 target_FL = -300; 00664 target_RL = 2000; 00665 target_RR = 300; 00666 masterSend.lowerRoller = true; 00667 masterSend.higherRoller = false; 00668 masterSend.rollerHeight = SHITA; 00669 zyouge = 2; 00670 twin_flag = true; 00671 }else if(GO_2400up && !GO_2400down) 00672 { 00673 move_flag = true; 00674 Rota_FR.reset(); 00675 Rota_FL.reset(); 00676 Rota_RL.reset(); 00677 Rota_RR.reset(); 00678 target_FR = -2000; 00679 target_FL = -300; 00680 target_RL = 2000; 00681 target_RR = 300; 00682 masterSend.lowerRoller = false; 00683 masterSend.higherRoller = true; 00684 masterSend.rollerHeight = SHITA; 00685 zyouge = 2; 00686 twin_flag = true; 00687 }else if(GO_2400up||GO_2400down) 00688 { 00689 move_flag = true; 00690 Rota_FR.reset(); 00691 Rota_FL.reset(); 00692 Rota_RL.reset(); 00693 Rota_RR.reset(); 00694 target_FR = -2000; 00695 target_FL = -300; 00696 target_RL = 2000; 00697 target_RR = 300; 00698 masterSend.lowerRoller = true; 00699 masterSend.higherRoller = true; 00700 masterSend.rollerHeight = SHITA; 00701 zyouge = 2; 00702 twin_flag = true; 00703 } 00704 } 00705 } 00706 00707 if(bluezone) 00708 { 00709 if(twin_flag) 00710 { 00711 if(move_flag) 00712 { 00713 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00714 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00715 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00716 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00717 motor[TIRE_FR].pwm = SetPWM(pwm.FR); 00718 motor[TIRE_FL].pwm = SetPWM(pwm.FL); 00719 motor[TIRE_RL].pwm = SetPWM(pwm.RL); 00720 motor[TIRE_RR].pwm = SetPWM(pwm.RR); 00721 } 00722 if(dist.FL > target_FL - 5)current = 1; 00723 } 00724 if(idoutable_flag) 00725 { 00726 if(move_flag){ 00727 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00728 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00729 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00730 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00731 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8; 00732 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3; 00733 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8; 00734 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3; 00735 } 00736 if(dist.FR > target_FR - 100) current = 4; 00737 } 00738 } 00739 if(redzone) 00740 { 00741 if(twin_flag) 00742 { 00743 if(move_flag) 00744 { 00745 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00746 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00747 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00748 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00749 motor[TIRE_FR].pwm = SetPWM(pwm.FR); 00750 motor[TIRE_FL].pwm = SetPWM(pwm.FL); 00751 motor[TIRE_RL].pwm = SetPWM(pwm.RL); 00752 motor[TIRE_RR].pwm = SetPWM(pwm.RR); 00753 } 00754 if(dist.RL > target_RL - 5)current = 1; 00755 } 00756 if(idoutable_flag) 00757 { 00758 if(move_flag){ 00759 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00760 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00761 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00762 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00763 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3; 00764 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8; 00765 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3; 00766 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8; 00767 } 00768 if(dist.RR > target_RR - 100) current = 4; 00769 } 00770 } 00771 00772 if(controller->Button.B){ 00773 AllBrake(); 00774 Rota_FR.reset(); 00775 Rota_FL.reset(); 00776 Rota_RL.reset(); 00777 Rota_RR.reset(); 00778 move_flag = false; 00779 } 00780 } 00781 #endif 00782 00783 #if USE_PROCESS_NUM>1 00784 static void Process1()//二段テーブルの位置合わせ 00785 { 00786 move_flag = false; 00787 ColorDetection(); 00788 Color_Changeflag(); 00789 /* 00790 pc.printf("FR: red %d g %d b %d \n\r",Color_FR[0],Color_FR[1],Color_FR[2]); 00791 pc.printf("FL: red %d g %d b %d \n\r",Color_FL[0],Color_FL[1],Color_FL[2]); 00792 pc.printf("RL: red %d g %d b %d \n\r",Color_RL[0],Color_RL[1],Color_RL[2]); 00793 pc.printf("RR: red %d g %d b %d \n\r",Color_RR[0],Color_RR[1],Color_RR[2]); 00794 */ 00795 if(!flag) 00796 { 00797 if(LS_FL && LS_FR) 00798 { 00799 AllBrake(); 00800 flag = true; 00801 } else if(!LS_FL && LS_FR) { 00802 GoFront(); 00803 }else if(LS_FL && !LS_FR) { 00804 GoFront(); 00805 }else if(!LS_FL && !LS_FR){ 00806 GoFront(); 00807 } 00808 } 00809 if(flag){ 00810 Move_On_the_Line(); 00811 } 00812 if(on_line_flag) 00813 { 00814 ugokanaide_flag = true; 00815 } 00816 if(ugokanaide_flag) 00817 { 00818 if(LS_FL && LS_FR) 00819 { 00820 masterSend.launch = true; 00821 AllBrake(); 00822 if(roller_flag) 00823 { 00824 launchTimer.attach(launchTimer_func, 7); 00825 roller_flag = false; 00826 } 00827 } else if(!LS_FL && LS_FR) { 00828 GoFront(); 00829 }else if(LS_FL && !LS_FR) { 00830 GoFront(); 00831 }else if(!LS_FL && !LS_FR){ 00832 GoFront(); 00833 } 00834 } 00835 00836 if(controller->Button.B){ 00837 AllBrake(); 00838 Rota_FR.reset(); 00839 Rota_FL.reset(); 00840 Rota_RL.reset(); 00841 Rota_RR.reset(); 00842 move_flag = false; 00843 } 00844 } 00845 #endif 00846 00847 #if USE_PROCESS_NUM>2 00848 static void Process2()//二段テーブル終了後ろへ or スタートゾーンへ戻る 00849 { 00850 masterSend.higherRoller = false; 00851 move_flag = false; 00852 if(GO_1200 || GO_1500 || GO_1800) 00853 { 00854 if(LS_RL && LS_RR) 00855 { 00856 Rota_FR.reset(); 00857 Rota_FL.reset(); 00858 Rota_RL.reset(); 00859 Rota_RR.reset(); 00860 AllBrake(); 00861 target_FR = 0; 00862 target_FL = 0; 00863 target_RL = 0; 00864 target_RR = 0; 00865 dist.FR = 0; 00866 dist.FL = 0; 00867 dist.RL = 0; 00868 dist.RR = 0; 00869 rotarota_flag = false; 00870 current = 3; 00871 }else if(!LS_RL && LS_RR) 00872 { 00873 motor[TIRE_FR].dir = FOR; 00874 motor[TIRE_FL].dir = FOR; 00875 motor[TIRE_RL].dir = FOR; 00876 motor[TIRE_RR].dir = FOR; 00877 motor[TIRE_FR].pwm = 0; 00878 motor[TIRE_FL].pwm = 60; 00879 motor[TIRE_RL].pwm = 0; 00880 motor[TIRE_RR].pwm = 0; 00881 }else if(LS_RL && !LS_RR) 00882 { 00883 motor[TIRE_FR].dir = BACK; 00884 motor[TIRE_FL].dir = BACK; 00885 motor[TIRE_RL].dir = BACK; 00886 motor[TIRE_RR].dir = BACK; 00887 motor[TIRE_FR].pwm = 60; 00888 motor[TIRE_FL].pwm = 0; 00889 motor[TIRE_RL].pwm = 0; 00890 motor[TIRE_RR].pwm = 0; 00891 }else if(!LS_RL && !LS_RR){ 00892 GoBack_Rota(); 00893 } 00894 } 00895 if(bluezone) 00896 { 00897 if(!GO_1200 && !GO_1500 && !GO_1800) 00898 { 00899 if(itido_flag) 00900 { 00901 Rota_FR.reset(); 00902 Rota_FL.reset(); 00903 Rota_RL.reset(); 00904 Rota_RR.reset(); 00905 target_FR = -550; 00906 target_FL = -2550; 00907 target_RL = 550; 00908 target_RR = 2550; 00909 itido_flag = false; 00910 } 00911 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00912 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00913 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00914 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00915 motor[TIRE_FR].pwm = SetPWM(pwm.FR); 00916 motor[TIRE_FL].pwm = SetPWM(pwm.FL); 00917 motor[TIRE_RL].pwm = SetPWM(pwm.RL); 00918 motor[TIRE_RR].pwm = SetPWM(pwm.RR); 00919 } 00920 }else if(redzone) 00921 { 00922 if(!GO_1200 && !GO_1500 && !GO_1800) 00923 { 00924 if(itido_flag) 00925 { 00926 Rota_FR.reset(); 00927 Rota_FL.reset(); 00928 Rota_RL.reset(); 00929 Rota_RR.reset(); 00930 00931 target_FR = 2650; 00932 target_FL = 400; 00933 target_RL = -2650; 00934 target_RR = -400; 00935 itido_flag = false; 00936 } 00937 motor[TIRE_FR].dir = SetStatus(pwm.FR); 00938 motor[TIRE_FL].dir = SetStatus(pwm.FL); 00939 motor[TIRE_RL].dir = SetStatus(pwm.RL); 00940 motor[TIRE_RR].dir = SetStatus(pwm.RR); 00941 motor[TIRE_FR].pwm = SetPWM(pwm.FR); 00942 motor[TIRE_FL].pwm = SetPWM(pwm.FL); 00943 motor[TIRE_RL].pwm = SetPWM(pwm.RL); 00944 motor[TIRE_RR].pwm = SetPWM(pwm.RR); 00945 } 00946 } 00947 } 00948 #endif 00949 00950 #if USE_PROCESS_NUM>3 00951 static void Process3()//移動テーブルのライン手前までロタコンで移動 00952 { 00953 if(!rotarota_flag) 00954 { 00955 if(bluezone) 00956 { 00957 if(GO_1200) 00958 { 00959 00960 Rota_FR.reset(); 00961 Rota_FL.reset(); 00962 Rota_RL.reset(); 00963 Rota_RR.reset(); 00964 rotarota_flag = true; 00965 target_FR = 1000; 00966 target_FL = 900; 00967 target_RL = -1000; 00968 target_RR = -900; 00969 masterSend.lowerRoller = true; 00970 masterSend.rollerHeight = SHITA; 00971 zyouge = 2; 00972 }else if(!GO_1200 && GO_1500) 00973 { 00974 Rota_FR.reset(); 00975 Rota_FL.reset(); 00976 Rota_RL.reset(); 00977 Rota_RR.reset(); 00978 rotarota_flag = true; 00979 target_FR = 1600; 00980 target_FL = 1500; 00981 target_RL = -1600; 00982 target_RR = -1500; 00983 masterSend.lowerRoller = true; 00984 masterSend.rollerHeight = MANNAKA; 00985 zyouge = 3; 00986 }else if(!GO_1200 && !GO_1500 && GO_1800) 00987 { 00988 Rota_FR.reset(); 00989 Rota_FL.reset(); 00990 Rota_RL.reset(); 00991 Rota_RR.reset(); 00992 rotarota_flag = true; 00993 target_FR = 2800; 00994 target_FL = 2700; 00995 target_RL = -2800; 00996 target_RR = -2700; 00997 masterSend.lowerRoller = true; 00998 masterSend.rollerHeight = UE; 00999 zyouge = 4; 01000 } 01001 }else if(redzone) 01002 { 01003 if(GO_1200) 01004 { 01005 Rota_FR.reset(); 01006 Rota_FL.reset(); 01007 Rota_RL.reset(); 01008 Rota_RR.reset(); 01009 rotarota_flag = true; 01010 target_FR = -900; 01011 target_FL = -1000; 01012 target_RL = 900; 01013 target_RR = 1000; 01014 masterSend.lowerRoller = true; 01015 masterSend.rollerHeight = SHITA; 01016 zyouge = 2; 01017 }else if(!GO_1200 && GO_1500) 01018 { 01019 Rota_FR.reset(); 01020 Rota_FL.reset(); 01021 Rota_RL.reset(); 01022 Rota_RR.reset(); 01023 rotarota_flag = true; 01024 target_FR = -1500; 01025 target_FL = -1600; 01026 target_RL = 1500; 01027 target_RR = 1600; 01028 masterSend.lowerRoller = true; 01029 masterSend.rollerHeight = MANNAKA; 01030 zyouge = 3; 01031 }else if(!GO_1200 && !GO_1500 && GO_1800) 01032 { 01033 Rota_FR.reset(); 01034 Rota_FL.reset(); 01035 Rota_RL.reset(); 01036 Rota_RR.reset(); 01037 rotarota_flag = true; 01038 target_FR = -2700; 01039 target_FL = -2800; 01040 target_RL = 2700; 01041 target_RR = 2800; 01042 masterSend.lowerRoller = true; 01043 masterSend.rollerHeight = UE; 01044 zyouge = 4; 01045 } 01046 } 01047 } 01048 if(bluezone) 01049 { 01050 if(rotarota_flag) 01051 { 01052 motor[TIRE_FR].dir = SetStatus(pwm.FR); 01053 motor[TIRE_FL].dir = SetStatus(pwm.FL); 01054 motor[TIRE_RL].dir = SetStatus(pwm.RL); 01055 motor[TIRE_RR].dir = SetStatus(pwm.RR); 01056 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8; 01057 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3; 01058 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8; 01059 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3; 01060 if(dist.FR > target_FR-100) 01061 { 01062 current = 4; 01063 } 01064 } 01065 }else if(redzone) 01066 { 01067 if(rotarota_flag) 01068 { 01069 motor[TIRE_FR].dir = SetStatus(pwm.FR); 01070 motor[TIRE_FL].dir = SetStatus(pwm.FL); 01071 motor[TIRE_RL].dir = SetStatus(pwm.RL); 01072 motor[TIRE_RR].dir = SetStatus(pwm.RR); 01073 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3; 01074 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8; 01075 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3; 01076 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8; 01077 if(dist.RR > (target_RR-100)) 01078 { 01079 current = 4; 01080 } 01081 } 01082 } 01083 } 01084 #endif 01085 01086 #if USE_PROCESS_NUM>4 01087 static void Process4()//ライン位置合わせ 01088 { 01089 rotarota_flag = false; 01090 masterSend.launch = false; 01091 roller_flag = true; 01092 once_flag = true; 01093 ColorDetection(); 01094 Color_Changeflag(); 01095 if (debug_flag) 01096 { 01097 on_line_flag = false; 01098 invation_FR = false; 01099 invation_FL = false; 01100 invation_RL = false; 01101 invation_RR = false; 01102 flag = false; 01103 ok_flag = false; 01104 debug_flag = false; 01105 //ushiroate_flag = false; 01106 } 01107 Move_On_the_Line_BackTrace(); 01108 if(on_line_flag) 01109 { 01110 if(LS_RL && LS_RR) 01111 { 01112 current = 5; 01113 ok_flag = true; 01114 AllBrake(); 01115 }else if(!LS_RL && LS_RR) 01116 { 01117 motor[TIRE_FR].dir = FOR; 01118 motor[TIRE_FL].dir = FOR; 01119 motor[TIRE_RL].dir = FOR; 01120 motor[TIRE_RR].dir = FOR; 01121 motor[TIRE_FR].pwm = 0; 01122 motor[TIRE_FL].pwm = 40; 01123 motor[TIRE_RL].pwm = 0; 01124 motor[TIRE_RR].pwm = 0; 01125 }else if(LS_RL && !LS_RR) 01126 { 01127 motor[TIRE_FR].dir = BACK; 01128 motor[TIRE_FL].dir = BACK; 01129 motor[TIRE_RL].dir = BACK; 01130 motor[TIRE_RR].dir = BACK; 01131 motor[TIRE_FR].pwm = 40; 01132 motor[TIRE_FL].pwm = 0; 01133 motor[TIRE_RL].pwm = 0; 01134 motor[TIRE_RR].pwm = 0; 01135 }else{ 01136 GoBack(); 01137 } 01138 } 01139 01140 if(controller->Button.B){ 01141 AllBrake(); 01142 Rota_FR.reset(); 01143 Rota_FL.reset(); 01144 Rota_RL.reset(); 01145 Rota_RR.reset(); 01146 move_flag = false; 01147 } 01148 } 01149 #endif 01150 01151 #if USE_PROCESS_NUM>5 01152 static void Process5()//最初の移動テーブルへライントレースし、投射 01153 { 01154 if(LS_FL && LS_FR) 01155 { 01156 masterSend.launch = true; 01157 rotarota_flag = false; 01158 AllBrake(); 01159 move_flag = false; 01160 if(roller_flag) 01161 { 01162 launchTimer.attach(launchTimer_func1, 3); 01163 roller_flag = false; 01164 } 01165 }else if(!LS_FL && LS_FR) { 01166 GoOshituke(); 01167 }else if(LS_FL && !LS_FR) { 01168 GoOshituke(); 01169 }else if(!LS_FL && !LS_FR){ 01170 Move_On_the_Line_Trace(); 01171 } 01172 } 01173 #endif 01174 01175 #if USE_PROCESS_NUM>6//下がる 01176 static void Process6() 01177 { 01178 if(LS_RL && LS_RR) { 01179 Rota_FR.reset(); 01180 Rota_FL.reset(); 01181 Rota_RL.reset(); 01182 Rota_RR.reset(); 01183 AllBrake(); 01184 rotarota_flag = false; 01185 target_FR = 0; 01186 target_FL = 0; 01187 target_RL = 0; 01188 target_RR = 0; 01189 dist.FR = 0; 01190 dist.FL = 0; 01191 dist.RL = 0; 01192 dist.RR = 0; 01193 current = 7; 01194 }else if(!LS_RL && LS_RR) { 01195 motor[TIRE_FR].dir = FOR; 01196 motor[TIRE_FL].dir = FOR; 01197 motor[TIRE_RL].dir = FOR; 01198 motor[TIRE_RR].dir = FOR; 01199 motor[TIRE_FR].pwm = 0; 01200 motor[TIRE_FL].pwm = 40; 01201 motor[TIRE_RL].pwm = 0; 01202 motor[TIRE_RR].pwm = 0; 01203 }else if(LS_RL && !LS_RR) { 01204 motor[TIRE_FR].dir = BACK; 01205 motor[TIRE_FL].dir = BACK; 01206 motor[TIRE_RL].dir = BACK; 01207 motor[TIRE_RR].dir = BACK; 01208 motor[TIRE_FR].pwm = 40; 01209 motor[TIRE_FL].pwm = 0; 01210 motor[TIRE_RL].pwm = 0; 01211 motor[TIRE_RR].pwm = 0; 01212 }else if(!LS_RL && !LS_RR){ 01213 GoBack_Rota(); 01214 } 01215 } 01216 #endif 01217 01218 #if USE_PROCESS_NUM>7 01219 static void Process7()//次の移動テーブルのライン手前までロタコンで移動 or スタートゾーンへ戻る 01220 { 01221 masterSend.launch = false; 01222 debug_flag1 = true; 01223 if(!rotarota_flag) 01224 { 01225 if(bluezone) 01226 { 01227 if(zyouge==2) 01228 { 01229 if(GO_1500) 01230 { 01231 masterSend.rollerHeight = MANNAKA; 01232 Rota_FR.reset(); 01233 Rota_FL.reset(); 01234 Rota_RL.reset(); 01235 Rota_RR.reset(); 01236 rotarota_flag = true; 01237 target_FR = 400; 01238 target_FL = 300; 01239 target_RL = -400; 01240 target_RR = -300; 01241 zyouge = 3; 01242 }else if(!GO_1500 && GO_1800) 01243 { 01244 masterSend.rollerHeight = UE; 01245 Rota_FR.reset(); 01246 Rota_FL.reset(); 01247 Rota_RL.reset(); 01248 Rota_RR.reset(); 01249 rotarota_flag = true; 01250 target_FR = 1100; 01251 target_FL = 1000; 01252 target_RL = -1100; 01253 target_RR = -1000; 01254 zyouge = 4; 01255 }else if(!GO_1500 && !GO_1800) 01256 { 01257 masterSend.rollerHeight = SHITA; 01258 kaeru_flag = true; 01259 } 01260 }else if(zyouge==3){ 01261 if(GO_1800) 01262 { 01263 masterSend.rollerHeight = UE; 01264 Rota_FR.reset(); 01265 Rota_FL.reset(); 01266 Rota_RL.reset(); 01267 Rota_RR.reset(); 01268 rotarota_flag = true; 01269 target_FR = 400; 01270 target_FL = 300; 01271 target_RL = -400; 01272 target_RR = -300; 01273 zyouge = 4; 01274 }else if(!GO_1800) 01275 { 01276 masterSend.rollerHeight = MANNAKA; 01277 kaeru_flag = true; 01278 } 01279 }else if(zyouge==4){ 01280 masterSend.rollerHeight = MANNAKA; 01281 kaeru_flag = true; 01282 } 01283 }else if(redzone) 01284 { 01285 if(zyouge==2) 01286 { 01287 if(GO_1500) 01288 { 01289 masterSend.rollerHeight = MANNAKA; 01290 Rota_FR.reset(); 01291 Rota_FL.reset(); 01292 Rota_RL.reset(); 01293 Rota_RR.reset(); 01294 rotarota_flag = true; 01295 target_FR = -300; 01296 target_FL = -400; 01297 target_RL = 300; 01298 target_RR = 400; 01299 zyouge = 3; 01300 }else if(!GO_1500 && GO_1800) 01301 { 01302 masterSend.rollerHeight = UE; 01303 Rota_FR.reset(); 01304 Rota_FL.reset(); 01305 Rota_RL.reset(); 01306 Rota_RR.reset(); 01307 rotarota_flag = true; 01308 target_FR = -1000; 01309 target_FL = -1100; 01310 target_RL = 1000; 01311 target_RR = 1100; 01312 zyouge = 4; 01313 }else if(!GO_1500 && !GO_1800) 01314 { 01315 masterSend.rollerHeight = MANNAKA; 01316 kaeru_flag = true; 01317 } 01318 }else if(zyouge==3){ 01319 if(GO_1800) 01320 { 01321 masterSend.rollerHeight = UE; 01322 Rota_FR.reset(); 01323 Rota_FL.reset(); 01324 Rota_RL.reset(); 01325 Rota_RR.reset(); 01326 01327 rotarota_flag = true; 01328 target_FR = -300; 01329 target_FL = -400; 01330 target_RL = 300; 01331 target_RR = 400; 01332 zyouge = 4; 01333 }else if(!GO_1800) 01334 { 01335 masterSend.rollerHeight = MANNAKA; 01336 kaeru_flag = true; 01337 } 01338 }else if(zyouge==4){ 01339 masterSend.rollerHeight = MANNAKA; 01340 kaeru_flag = true; 01341 } 01342 } 01343 } 01344 if(bluezone) 01345 { 01346 if(rotarota_flag) 01347 { 01348 motor[TIRE_FR].dir = SetStatus(pwm.FR); 01349 motor[TIRE_FL].dir = SetStatus(pwm.FL); 01350 motor[TIRE_RL].dir = SetStatus(pwm.RL); 01351 motor[TIRE_RR].dir = SetStatus(pwm.RR); 01352 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8; 01353 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3; 01354 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8; 01355 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3; 01356 if(dist.FR > (target_FR-100)) 01357 { 01358 current = 8; 01359 } 01360 } 01361 if(kaeru_flag) 01362 { 01363 motor[TIRE_FR].dir = FOR; 01364 motor[TIRE_FL].dir = FOR; 01365 motor[TIRE_RL].dir = BACK; 01366 motor[TIRE_RR].dir = BACK; 01367 motor[TIRE_FR].pwm = 40; 01368 motor[TIRE_FL].pwm = 100; 01369 motor[TIRE_RL].pwm = 40; 01370 motor[TIRE_RR].pwm = 100; 01371 masterSend.rollerHeight = MANNAKA; 01372 } 01373 }else if(redzone) 01374 { 01375 if(rotarota_flag) 01376 { 01377 motor[TIRE_FR].dir = SetStatus(pwm.FR); 01378 motor[TIRE_FL].dir = SetStatus(pwm.FL); 01379 motor[TIRE_RL].dir = SetStatus(pwm.RL); 01380 motor[TIRE_RR].dir = SetStatus(pwm.RR); 01381 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3; 01382 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8; 01383 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3; 01384 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8; 01385 if(dist.RR > (target_RR-100)) 01386 { 01387 current = 8; 01388 } 01389 } 01390 if(kaeru_flag) 01391 { 01392 motor[TIRE_FR].dir = BACK; 01393 motor[TIRE_FL].dir = BACK; 01394 motor[TIRE_RL].dir = FOR; 01395 motor[TIRE_RR].dir = FOR; 01396 motor[TIRE_FR].pwm = 100; 01397 motor[TIRE_FL].pwm = 40; 01398 motor[TIRE_RL].pwm = 100; 01399 motor[TIRE_RR].pwm = 40; 01400 masterSend.rollerHeight = MANNAKA; 01401 } 01402 } 01403 01404 01405 } 01406 #endif 01407 01408 #if USE_PROCESS_NUM>8 01409 static void Process8()//ライン位置合わせ 01410 { 01411 roller_flag = true; 01412 ColorDetection(); 01413 Color_Changeflag(); 01414 01415 if (debug_flag1) 01416 { 01417 //zyouge++; 01418 on_line_flag = false; 01419 invation_FR = false; 01420 invation_FL = false; 01421 invation_RL = false; 01422 invation_RR = false; 01423 debug_flag1 = false; 01424 ugoite_flag = false; 01425 flag = false; 01426 temae_flag = false; 01427 ok_flag = false; 01428 } 01429 01430 Move_On_the_Line_BackTrace(); 01431 01432 if(on_line_flag) 01433 { 01434 if(LS_RL && LS_RR) 01435 { 01436 AllBrake(); 01437 current = 9; 01438 ok_flag = true; 01439 }else if(!LS_RL && LS_RR) 01440 { 01441 motor[TIRE_FR].dir = FOR; 01442 motor[TIRE_FL].dir = FOR; 01443 motor[TIRE_RL].dir = FOR; 01444 motor[TIRE_RR].dir = FOR; 01445 motor[TIRE_FR].pwm = 0; 01446 motor[TIRE_FL].pwm = 40; 01447 motor[TIRE_RL].pwm = 0; 01448 motor[TIRE_RR].pwm = 0; 01449 }else if(LS_RL && !LS_RR) 01450 { 01451 motor[TIRE_FR].dir = BACK; 01452 motor[TIRE_FL].dir = BACK; 01453 motor[TIRE_RL].dir = BACK; 01454 motor[TIRE_RR].dir = BACK; 01455 motor[TIRE_FR].pwm = 40; 01456 motor[TIRE_FL].pwm = 0; 01457 motor[TIRE_RL].pwm = 0; 01458 motor[TIRE_RR].pwm = 0; 01459 }else{ 01460 GoBack(); 01461 } 01462 } 01463 01464 if(controller->Button.B){ 01465 AllBrake(); 01466 Rota_FR.reset(); 01467 Rota_FL.reset(); 01468 Rota_RL.reset(); 01469 Rota_RR.reset(); 01470 move_flag = false; 01471 } 01472 } 01473 #endif 01474 01475 #if USE_PROCESS_NUM>9 01476 static void Process9()//ライントレースし、投射->Process6へ戻る 01477 { 01478 once_flag = true; 01479 rotarota_flag = false; 01480 move_flag = false; 01481 Move_On_the_Line_Trace(); 01482 if(LS_FL && LS_FR) 01483 { 01484 target_FR = 0; 01485 target_FL = 0; 01486 target_RL = 0; 01487 target_RR = 0; 01488 dist.FR = 0; 01489 dist.FL = 0; 01490 dist.RL = 0; 01491 dist.RR = 0; 01492 01493 masterSend.launch = true; 01494 AllBrake(); 01495 move_flag = false; 01496 if(roller_flag) 01497 { 01498 launchTimer.attach(launchTimer_func2, 3); 01499 roller_flag = false; 01500 } 01501 }else if(!LS_FL && LS_FR) { 01502 GoOshituke(); 01503 }else if(LS_FL && !LS_FR) { 01504 GoOshituke(); 01505 } 01506 } 01507 #endif 01508 #endif 01509 #pragma endregion PROCESS 01510 01511 static void AllActuatorReset() 01512 { 01513 01514 #ifdef USE_SOLENOID 01515 solenoid.all = ALL_SOLENOID_OFF; 01516 #endif 01517 01518 #ifdef USE_MOTOR 01519 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 01520 motor[i].dir = FREE; 01521 motor[i].pwm = 0x00; 01522 } 01523 #endif 01524 } 01525 01526 #pragma region USER-DEFINED-FUNCTIONS 01527 01528 void Get_Move() 01529 { 01530 rps.FR = (rotate.FR-p_rps.FR)/0.1; 01531 rps.FL = (rotate.FL-p_rps.FL)/0.1; 01532 rps.RL = (rotate.RL-p_rps.RL)/0.1; 01533 rps.RR = (rotate.RR-p_rps.RR)/0.1; 01534 pwm_FR = (int)Catch_pwm_FR.SetPV(rps.FR , wantVal_FR); 01535 pwm_FL = (int)Catch_pwm_FL.SetPV(rps.FL , wantVal_FL); 01536 pwm_RL = (int)Catch_pwm_RL.SetPV(rps.RL , wantVal_RL); 01537 pwm_RR = (int)Catch_pwm_RR.SetPV(rps.RR , wantVal_RR); 01538 01539 p_rps.FR = rotate.FR; 01540 p_rps.FL = rotate.FL; 01541 p_rps.RL = rotate.RL; 01542 p_rps.RR = rotate.RR; 01543 } 01544 01545 void AllBrake() 01546 { 01547 motor[TIRE_FR].dir = BRAKE; 01548 motor[TIRE_FL].dir = BRAKE; 01549 motor[TIRE_RL].dir = BRAKE; 01550 motor[TIRE_RR].dir = BRAKE; 01551 motor[TIRE_FR].pwm = 255; 01552 motor[TIRE_FL].pwm = 255; 01553 motor[TIRE_RL].pwm = 255; 01554 motor[TIRE_RR].pwm = 255; 01555 } 01556 01557 void GoOshituke() 01558 { 01559 motor[TIRE_FR].dir = FOR; 01560 motor[TIRE_FL].dir = BACK; 01561 motor[TIRE_RL].dir = BACK; 01562 motor[TIRE_RR].dir = FOR; 01563 motor[TIRE_FR].pwm = 48; 01564 motor[TIRE_FL].pwm = 48; 01565 motor[TIRE_RL].pwm = 48; 01566 motor[TIRE_RR].pwm = 48; 01567 } 01568 void GoFront() 01569 { 01570 motor[TIRE_FR].dir = FOR; 01571 motor[TIRE_FL].dir = BACK; 01572 motor[TIRE_RL].dir = BACK; 01573 motor[TIRE_RR].dir = FOR; 01574 motor[TIRE_FR].pwm = 48; 01575 motor[TIRE_FL].pwm = 48; 01576 motor[TIRE_RL].pwm = 48; 01577 motor[TIRE_RR].pwm = 48; 01578 } 01579 01580 void GoMae() 01581 { 01582 motor[TIRE_FR].dir = FOR; 01583 motor[TIRE_FL].dir = BACK; 01584 motor[TIRE_RL].dir = BACK; 01585 motor[TIRE_RR].dir = FOR; 01586 motor[TIRE_FR].pwm = 29; 01587 motor[TIRE_FL].pwm = 32; 01588 motor[TIRE_RL].pwm = 35; 01589 motor[TIRE_RR].pwm = 37; 01590 01591 } 01592 void GoBack() 01593 { 01594 motor[TIRE_FR].dir = BACK; 01595 motor[TIRE_FL].dir = FOR; 01596 motor[TIRE_RL].dir = FOR; 01597 motor[TIRE_RR].dir = BACK; 01598 motor[TIRE_FR].pwm = 25; 01599 motor[TIRE_FL].pwm = 22; 01600 motor[TIRE_RL].pwm = 22; 01601 motor[TIRE_RR].pwm = 25; 01602 } 01603 01604 void GoBack_Rota() 01605 { 01606 if(once_flag) 01607 { 01608 dist.FR = 0; 01609 dist.FL = 0; 01610 dist.RL = 0; 01611 dist.RR = 0; 01612 Rota_FR.reset(); 01613 Rota_FL.reset(); 01614 Rota_RL.reset(); 01615 Rota_RR.reset(); 01616 target_FR = 8000; 01617 target_FL = -8000; 01618 target_RL = -8000; 01619 target_RR = 8000; 01620 once_flag = false; 01621 } 01622 01623 motor[TIRE_FR].dir = SetStatus(pwm.FR); 01624 motor[TIRE_FL].dir = SetStatus(pwm.FL); 01625 motor[TIRE_RL].dir = SetStatus(pwm.RL); 01626 motor[TIRE_RR].dir = SetStatus(pwm.RR); 01627 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.51; 01628 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.52; 01629 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.53; 01630 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.47; 01631 01632 } 01633 01634 void GoLeft() 01635 { 01636 motor[TIRE_FR].dir = FOR; 01637 motor[TIRE_FL].dir = FOR; 01638 motor[TIRE_RL].dir = BACK; 01639 motor[TIRE_RR].dir = BACK; 01640 motor[TIRE_FR].pwm = 24; 01641 motor[TIRE_FL].pwm = 24; 01642 motor[TIRE_RL].pwm = 24; 01643 motor[TIRE_RR].pwm = 24; 01644 } 01645 01646 void GoMigiNanameMae() 01647 { 01648 motor[TIRE_FR].dir = FOR; 01649 motor[TIRE_FL].dir = BACK; 01650 motor[TIRE_RL].dir = BACK; 01651 motor[TIRE_RR].dir = FOR; 01652 motor[TIRE_FR].pwm = 23; 01653 motor[TIRE_FL].pwm = 35; 01654 motor[TIRE_RL].pwm = 23; 01655 motor[TIRE_RR].pwm = 30; 01656 } 01657 01658 void GoMigiNanameUshiro() 01659 { 01660 motor[TIRE_FR].dir = BACK; 01661 motor[TIRE_FL].dir = BACK; 01662 motor[TIRE_RL].dir = FOR; 01663 motor[TIRE_RR].dir = FOR; 01664 motor[TIRE_FR].pwm = 30; 01665 motor[TIRE_FL].pwm = 0; 01666 motor[TIRE_RL].pwm = 30; 01667 motor[TIRE_RR].pwm = 0; 01668 } 01669 01670 void GoRight() 01671 { 01672 motor[TIRE_FR].dir = BACK; 01673 motor[TIRE_FL].dir = BACK; 01674 motor[TIRE_RL].dir = FOR; 01675 motor[TIRE_RR].dir = FOR; 01676 motor[TIRE_FR].pwm = 24; 01677 motor[TIRE_FL].pwm = 24; 01678 motor[TIRE_RL].pwm = 24; 01679 motor[TIRE_RR].pwm = 24; 01680 } 01681 01682 void GoHidariNanameMae() 01683 { 01684 motor[TIRE_FR].dir = FOR; 01685 motor[TIRE_FL].dir = BACK; 01686 motor[TIRE_RL].dir = BACK; 01687 motor[TIRE_RR].dir = FOR; 01688 motor[TIRE_FR].pwm = 30; 01689 motor[TIRE_FL].pwm = 23; 01690 motor[TIRE_RL].pwm = 35; 01691 motor[TIRE_RR].pwm = 23; 01692 } 01693 01694 void GoHidariNanameUshiro() 01695 { 01696 motor[TIRE_FR].dir = FOR; 01697 motor[TIRE_FL].dir = FOR; 01698 motor[TIRE_RL].dir = BACK; 01699 motor[TIRE_RR].dir = BACK; 01700 motor[TIRE_FR].pwm = 0; 01701 motor[TIRE_FL].pwm = 30; 01702 motor[TIRE_RL].pwm = 0; 01703 motor[TIRE_RR].pwm = 30; 01704 } 01705 01706 void RightTurning() 01707 { 01708 motor[TIRE_FR].dir = BACK; 01709 motor[TIRE_FL].dir = BACK; 01710 motor[TIRE_RL].dir = BACK; 01711 motor[TIRE_RR].dir = BACK; 01712 motor[TIRE_FR].pwm = 0; 01713 motor[TIRE_FL].pwm = 35; 01714 motor[TIRE_RL].pwm = 35; 01715 motor[TIRE_RR].pwm = 30; 01716 } 01717 01718 void LeftTurning() 01719 { 01720 motor[TIRE_FR].dir = FOR; 01721 motor[TIRE_FL].dir = FOR; 01722 motor[TIRE_RL].dir = FOR; 01723 motor[TIRE_RR].dir = FOR; 01724 motor[TIRE_FR].pwm = 30; 01725 motor[TIRE_FL].pwm = 0; 01726 motor[TIRE_RL].pwm = 35; 01727 motor[TIRE_RR].pwm = 30; 01728 } 01729 01730 void Get_rps() 01731 { 01732 rotate.FR = ((double)Rota_FR.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); 01733 rotate.FL = ((double)Rota_FL.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); 01734 rotate.RL = ((double)Rota_RL.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); 01735 rotate.RR = ((double)Rota_RR.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); 01736 01737 dist.FR = 103.0*3.141*rotate.FR; 01738 dist.FL = 103.0*3.141*rotate.FL; 01739 dist.RL = 103.0*3.141*rotate.RL; 01740 dist.RR = 103.0*3.141*rotate.RR; 01741 01742 pwm.FR = (int)Get_pwm_FR.SetPV(dist.FR , target_FR); 01743 pwm.FL = (int)Get_pwm_FL.SetPV(dist.FL , target_FL); 01744 pwm.RL = (int)Get_pwm_RL.SetPV(dist.RL , target_RL); 01745 pwm.RR = (int)Get_pwm_RR.SetPV(dist.RR , target_RR); 01746 } 01747 01748 void Rota_SetStatus(char position,double percent) 01749 { 01750 switch(position){ 01751 case TIRE_FR : 01752 wantVal_FR = (percent/100.0)*max_rotate; 01753 //pwm_FR = (int)Catch_pwm_FR.SetPV(rps.FR , wantVal_FR); 01754 motor[TIRE_FR].dir = SetStatus(pwm_FR); 01755 motor[TIRE_FR].pwm = SetPWM(pwm_FR); 01756 break; 01757 case TIRE_FL : 01758 wantVal_FL = (percent/100.0)*max_rotate; 01759 //pwm_FL = (int)Catch_pwm_FL.SetPV(rps.FL , wantVal_FL); 01760 motor[TIRE_FL].dir = SetStatus(pwm_FL); 01761 motor[TIRE_FL].pwm = SetPWM(pwm_FL); 01762 break; 01763 case TIRE_RL : 01764 wantVal_RL = (percent/100.0)*max_rotate; 01765 //pwm_RL = (int)Catch_pwm_RL.SetPV(rps.RL , wantVal_RL); 01766 motor[TIRE_RL].dir = SetStatus(pwm_RL); 01767 motor[TIRE_RL].pwm = SetPWM(pwm_RL); 01768 break; 01769 case TIRE_RR : 01770 wantVal_RR = (percent/100.0)*max_rotate; 01771 //pwm_RR = (int)Catch_pwm_RR.SetPV(rps.RR , wantVal_RR); 01772 motor[TIRE_RR].dir = SetStatus(pwm_RR); 01773 motor[TIRE_RR].pwm = SetPWM(pwm_RR); 01774 break; 01775 } 01776 } 01777 01778 uint8_t SetStatus(int pwmVal) 01779 { 01780 if(pwmVal < 0) return BACK; 01781 else if(pwmVal > 0) return FOR; 01782 else return BRAKE; 01783 } 01784 01785 uint8_t SetPWM(int pwmVal) 01786 { 01787 if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; 01788 else return abs(pwmVal); 01789 } 01790 unsigned long ColorIn(int index) 01791 { 01792 int result = 0; 01793 bool rtn = false; 01794 for(int i=0; i<12; i++) 01795 { 01796 CK = 1; 01797 rtn = DOUT[index]; 01798 CK = 0; 01799 if(rtn) 01800 { 01801 result|=(1 << i); 01802 } 01803 } 01804 return result; 01805 } 01806 01807 void ColorDetection(){ 01808 GATE = 0; 01809 01810 CK = 0; 01811 01812 RANGE = 1; 01813 01814 GATE = 1; 01815 wait_ms(intergration); 01816 GATE = 0; 01817 wait_us(4); 01818 switch(select++){ 01819 case 0 : 01820 Color_FR[0] = ColorIn(0); //赤 01821 wait_us(3); 01822 Color_FR[1] = ColorIn(0); //青 01823 wait_us(3); 01824 Color_FR[2] = ColorIn(0); //緑 01825 break; 01826 case 1 : 01827 Color_FL[0] = ColorIn(1); 01828 wait_us(3); 01829 Color_FL[1] = ColorIn(1); 01830 wait_us(3); 01831 Color_FL[2] = ColorIn(1); 01832 break; 01833 case 2 : 01834 Color_RL[0] = ColorIn(2); 01835 wait_us(3); 01836 Color_RL[1] = ColorIn(2); 01837 wait_us(3); 01838 Color_RL[2] = ColorIn(2); 01839 break; 01840 case 3 : 01841 Color_RR[0] = ColorIn(3); 01842 wait_us(3); 01843 Color_RR[1] = ColorIn(3); 01844 wait_us(3); 01845 Color_RR[2] = ColorIn(3); 01846 select = 0; 01847 break; 01848 } 01849 } 01850 01851 void Color_Changeflag(){ 01852 ColorDetection(); 01853 01854 if(Color_FR[0] > line_FR[0] && Color_FR[1] > line_FR[1] && Color_FR[2] > line_FR[2] && !white_FR)//白 01855 { 01856 //invation_FR ^= 1;//start false,over true 01857 white_FR = true;//on true,noon false 01858 } 01859 else if(!(Color_FR[0] > line_FR[0] && Color_FR[1] > line_FR[1] && Color_FR[2] > line_FR[2]))white_FR = false;//茶 01860 01861 if(Color_FL[0] > line_FL[0] && Color_FL[1] > line_FL[1] && Color_FL[2] > line_FL[2] && !white_FL)//白 01862 { 01863 //invation_FL ^= 1;//start false,over true 01864 white_FL = true;//on true,noon false 01865 } 01866 else if(!(Color_FL[0] > line_FL[0] && Color_FL[1] > line_FL[1] && Color_FL[2] > line_FL[2]))white_FL = false;//茶 01867 01868 if(Color_RL[0] > line_RL[0] && Color_RL[1] > line_RL[1] && Color_RL[2] > line_RL[2] && !white_RL)//白 01869 { 01870 //invation_RL ^= 1;//start false,over true 01871 white_RL = true;//on true,noon false 01872 } 01873 else if(!(Color_RL[0] > line_RL[0] && Color_RL[1] > line_RL[1] && Color_RL[2] > line_RL[2]))white_RL = false;//茶 01874 01875 if(Color_RR[0] > line_RR[0] && Color_RR[1] > line_RR[1] && Color_RR[2] > line_RR[2] && !white_RR)//白 01876 { 01877 //invation_RR ^= 1;//start false,over true 01878 white_RR = true;//on true,noon false 01879 } 01880 else if(!(Color_RR[0] > line_RR[0] && Color_RR[1] > line_RR[1] && Color_RR[2] > line_RR[2]))white_RR = false;//茶 01881 } 01882 01883 void Move_On_the_Line() 01884 { 01885 Color_Changeflag(); 01886 if(bluezone) 01887 { 01888 if(white_FL && !white_FR) 01889 { 01890 GoLeft(); 01891 invation_FL = false; 01892 invation_FR = true; 01893 on_line_flag = false; 01894 } 01895 if(!white_FL && white_FR) 01896 { 01897 GoRight(); 01898 invation_FL = false; 01899 invation_FR = false; 01900 on_line_flag = false; 01901 } 01902 if(!white_FL && !white_FR) 01903 { 01904 if(!white_RR) 01905 { 01906 if(invation_FR) 01907 { 01908 GoLeft(); 01909 on_line_flag = false; 01910 }else if(!invation_FR) 01911 { 01912 GoRight(); 01913 on_line_flag = false; 01914 } 01915 } 01916 } 01917 if(white_RR) 01918 { 01919 GoMae(); 01920 on_line_flag = true; 01921 } 01922 }else if(redzone) 01923 { 01924 01925 if(white_FL && !white_FR) 01926 { 01927 GoLeft(); 01928 invation_FL = false; 01929 invation_FR = false; 01930 on_line_flag = false; 01931 } 01932 if(white_FR && !white_FL) 01933 { 01934 GoRight(); 01935 invation_FL = true; 01936 invation_FR = false; 01937 on_line_flag = false; 01938 } 01939 if(!white_FL && !white_FR) 01940 { 01941 if(!white_RR) 01942 { 01943 if(invation_FL) 01944 { 01945 GoRight(); 01946 on_line_flag = false; 01947 }else if(!invation_FL) 01948 { 01949 GoLeft(); 01950 on_line_flag = false; 01951 } 01952 } 01953 } 01954 if(white_RR) 01955 { 01956 GoMae(); 01957 on_line_flag = true; 01958 } 01959 } 01960 } 01961 void Move_On_the_Line_Trace() 01962 { 01963 Color_Changeflag(); 01964 if(bluezone) 01965 { 01966 if(white_FL && !white_FR && !white_RR)//右に行きすぎ 01967 { 01968 invation_FR = true; 01969 invation_FL = false; 01970 invation_RR = true; 01971 GoHidariNanameMae(); 01972 on_line_flag = false; 01973 }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ 01974 { 01975 invation_FR = false; 01976 invation_FL = false; 01977 invation_RR = false; 01978 GoMigiNanameMae(); 01979 on_line_flag = false; 01980 }else if(!white_FL && !white_FR && !white_RR)//線の右か左 01981 { 01982 if(!invation_FL && !invation_FR && !invation_RR) 01983 { 01984 GoMigiNanameMae(); 01985 on_line_flag = false; 01986 01987 } 01988 if(invation_FL && invation_FR && invation_RR) 01989 { 01990 GoHidariNanameMae(); 01991 on_line_flag = false; 01992 } 01993 }else if(white_FL && !white_FR && white_RR)//ちょい右 01994 { 01995 invation_FR = true; 01996 invation_FL = false; 01997 invation_RR = false; 01998 GoHidariNanameMae(); 01999 on_line_flag = false; 02000 }else if(!white_FL && white_FR && white_RR)//ちょい左 02001 { 02002 invation_FR = true; 02003 invation_FL = false; 02004 invation_RR = true; 02005 GoMigiNanameMae(); 02006 on_line_flag = false; 02007 }else if(white_FL && white_FR && !white_RR)//機体が斜め 02008 { 02009 if(invation_RR) 02010 { 02011 GoMae(); 02012 on_line_flag = false; 02013 } 02014 if(!invation_RR) 02015 { 02016 GoMae(); 02017 on_line_flag = false; 02018 } 02019 }else if(white_FL && white_FR && white_RR)//十字の時 02020 { 02021 GoMae(); 02022 on_line_flag = true; 02023 }else if(!white_FL && !white_FR && white_RR)//OK 02024 { 02025 GoMae(); 02026 on_line_flag = true; 02027 } 02028 }else if(redzone) 02029 { 02030 if(white_FL && !white_FR && !white_RR)//右に行きすぎ 02031 { 02032 invation_FR = false; 02033 invation_FL = false; 02034 invation_RR = false; 02035 GoHidariNanameMae(); 02036 on_line_flag = false; 02037 }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ 02038 { 02039 invation_FR = false; 02040 invation_FL = true; 02041 invation_RR = true; 02042 GoMigiNanameMae(); 02043 on_line_flag = false; 02044 }else if(!white_FL && !white_FR && !white_RR)//線の右か左 02045 { 02046 if(!invation_FL && !invation_FR && !invation_RR) 02047 { 02048 GoHidariNanameMae(); 02049 on_line_flag = false; 02050 02051 } 02052 if(invation_FL && invation_FR && invation_RR) 02053 { 02054 GoMigiNanameMae(); 02055 on_line_flag = false; 02056 } 02057 }else if(white_FL && !white_FR && white_RR)//ちょい右 02058 { 02059 invation_FR = false; 02060 invation_FL = false; 02061 invation_RR = false; 02062 GoHidariNanameMae(); 02063 on_line_flag = false; 02064 }else if(!white_FL && white_FR && white_RR)//ちょい左 02065 { 02066 invation_FR = false; 02067 invation_FL = true; 02068 invation_RR = false; 02069 GoMigiNanameMae(); 02070 on_line_flag = false; 02071 }else if(white_FL && white_FR && !white_RR)//機体が斜め 02072 { 02073 if(invation_RR) 02074 { 02075 GoMae(); 02076 on_line_flag = false; 02077 } 02078 if(!invation_RR) 02079 { 02080 GoMae(); 02081 on_line_flag = false; 02082 } 02083 }else if(white_FL && white_FR && white_RR)//十字の時 02084 { 02085 GoMae(); 02086 on_line_flag = true; 02087 }else if(!white_FL && !white_FR && white_RR)//OK 02088 { 02089 GoMae(); 02090 on_line_flag = true; 02091 } 02092 } 02093 } 02094 02095 void Move_On_the_Line_BackTrace() 02096 { 02097 Color_Changeflag(); 02098 if(bluezone) 02099 { 02100 if(white_FL && !white_FR && !white_RR)//右に行きすぎ 02101 { 02102 invation_FR = true; 02103 invation_FL = false; 02104 invation_RR = true; 02105 GoHidariNanameUshiro(); 02106 on_line_flag = false; 02107 }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ 02108 { 02109 invation_FR = false; 02110 invation_FL = false; 02111 invation_RR = false; 02112 GoMigiNanameUshiro(); 02113 on_line_flag = false; 02114 }else if(!white_FL && !white_FR && !white_RR)//線の右か左 02115 { 02116 if(!invation_FL && !invation_FR && !invation_RR) 02117 { 02118 GoMigiNanameUshiro(); 02119 on_line_flag = false; 02120 02121 } 02122 if(invation_FL && invation_FR && invation_RR) 02123 { 02124 GoHidariNanameUshiro(); 02125 on_line_flag = false; 02126 } 02127 }else if(white_FL && !white_FR && white_RR)//ちょい右 02128 { 02129 invation_FR = true; 02130 invation_FL = false; 02131 invation_RR = false; 02132 GoHidariNanameUshiro(); 02133 on_line_flag = false; 02134 }else if(!white_FL && white_FR && white_RR)//ちょい左 02135 { 02136 invation_FR = true; 02137 invation_FL = false; 02138 invation_RR = true; 02139 GoMigiNanameUshiro(); 02140 on_line_flag = false; 02141 }else if(white_FL && white_FR && !white_RR)//機体が斜め 02142 { 02143 if(invation_RR) 02144 { 02145 RightTurning(); 02146 on_line_flag = false; 02147 } 02148 if(!invation_RR) 02149 { 02150 LeftTurning(); 02151 on_line_flag = false; 02152 } 02153 }else if(white_FL && white_FR && white_RR)//十字の時 02154 { 02155 GoBack(); 02156 on_line_flag = true; 02157 }else if(!white_FL && !white_FR && white_RR)//OK 02158 { 02159 GoBack(); 02160 on_line_flag = true; 02161 } 02162 }else if(redzone) 02163 { 02164 if(white_FL && !white_FR && !white_RR)//右に行きすぎ 02165 { 02166 invation_FR = false; 02167 invation_FL = false; 02168 invation_RR = false; 02169 GoHidariNanameUshiro(); 02170 on_line_flag = false; 02171 }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ 02172 { 02173 invation_FR = false; 02174 invation_FL = true; 02175 invation_RR = true; 02176 GoMigiNanameUshiro(); 02177 on_line_flag = false; 02178 }else if(!white_FL && !white_FR && !white_RR)//線の右か左 02179 { 02180 if(!invation_FL && !invation_FR && !invation_RR) 02181 { 02182 GoHidariNanameUshiro(); 02183 on_line_flag = false; 02184 02185 } 02186 if(invation_FL && invation_FR && invation_RR) 02187 { 02188 GoMigiNanameUshiro(); 02189 on_line_flag = false; 02190 } 02191 }else if(white_FL && !white_FR && white_RR)//ちょい右 02192 { 02193 invation_FR = false; 02194 invation_FL = false; 02195 invation_RR = false; 02196 GoHidariNanameUshiro(); 02197 on_line_flag = false; 02198 }else if(!white_FL && white_FR && white_RR)//ちょい左 02199 { 02200 invation_FR = false; 02201 invation_FL = true; 02202 invation_RR = false; 02203 GoMigiNanameUshiro(); 02204 on_line_flag = false; 02205 }else if(white_FL && white_FR && !white_RR)//機体が斜め 02206 { 02207 if(invation_RR) 02208 { 02209 LeftTurning(); 02210 on_line_flag = false; 02211 } 02212 if(!invation_RR) 02213 { 02214 RightTurning(); 02215 on_line_flag = false; 02216 } 02217 }else if(white_FL && white_FR && white_RR)//十字の時 02218 { 02219 GoBack(); 02220 on_line_flag = true; 02221 }else if(!white_FL && !white_FR && white_RR)//OK 02222 { 02223 GoBack(); 02224 on_line_flag = true; 02225 } 02226 } 02227 } 02228 void BuzzerTimer_func() 02229 { 02230 buzzer = !buzzer; 02231 } 02232 02233 void launchTimer_func() { 02234 02235 current = 2; 02236 } 02237 void launchTimer_func1(){ 02238 //masterSend.launch = true; 02239 current = 6; 02240 } 02241 void launchTimer_func2() 02242 { 02243 //masterSend.launch = true; 02244 current = 6; 02245 } 02246 #pragma endregion
Generated on Wed Jul 13 2022 00:47:24 by
1.7.2