aa
Dependencies: mbed TrapezoidControl QEI
Process.cpp
00001 00002 #include "mbed.h" 00003 #include "Process.h" 00004 #include "QEI.h" 00005 00006 #include "../../CommonLibraries/PID/PID.h" 00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" 00008 #include "../../Communication/RS485/LineHub/LineHub.h" 00009 #include "../../Communication/Controller/Controller.h" 00010 #include "../../Input/ExternalInt/ExternalInt.h" 00011 #include "../../Input/Switch/Switch.h" 00012 #include "../../Input/Potentiometer/Potentiometer.h" 00013 #include "../../Input/Encoder/Encoder.h" 00014 #include "../../LED/LED.h" 00015 #include "../../Safty/Safty.h" 00016 #include "../Using.h" 00017 00018 using namespace SWITCH; 00019 using namespace PID_SPACE; 00020 using namespace ENCODER; 00021 using namespace LINEHUB; 00022 00023 static CONTROLLER::ControllerData *controller; 00024 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00025 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; 00026 00027 static bool lock; 00028 static bool processChangeComp; 00029 static int current; 00030 00031 static void AllActuatorReset(); 00032 00033 #ifdef USE_SUBPROCESS 00034 static void (*Process[USE_PROCESS_NUM])(void); 00035 #endif 00036 00037 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE 00038 00039 /*Replace here with the definition code of your variables.*/ 00040 00041 Serial pc(USBTX, USBRX); 00042 00043 //**************Encoder*************** 00044 const int PerRev = 256; 00045 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING); 00046 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING); 00047 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING); 00048 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING); 00049 //**************Encoder*************** 00050 00051 //**************Buzzer**************** 00052 //DigitalOut buzzer(BUZZER_PIN); 00053 void BuzzerTimer_func(); 00054 Ticker BuzzerTimer; 00055 bool EMGflag = false; 00056 //PWMOut buzzer(BUZZER_PIN); 00057 //**************Buzzer**************** 00058 00059 //************TapeLed***************** 00060 void TapeLedEms_func(); 00061 TapeLedData tapeLED; 00062 TapeLedData sendLedData; 00063 TapeLED_Mode ledMode = Normal; 00064 Ticker tapeLedTimer; 00065 //************TapaLed***************** 00066 00067 float tireProRPM[4]; 00068 float tireTarRPM[4]; 00069 float tirepwm[4]; 00070 00071 const int omni[15][15] = { 00072 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, 00073 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, 00074 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, 00075 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, 00076 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, 00077 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, 00078 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, 00079 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, 00080 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, 00081 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, 00082 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, 00083 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, 00084 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, 00085 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, 00086 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } 00087 }; 00088 00089 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 }; 00090 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200} 00091 00092 uint8_t SetStatus(int); 00093 uint8_t SetStatus(int pwmVal) 00094 { 00095 if (pwmVal < 0) return BACK; 00096 else if (pwmVal > 0) return FOR; 00097 else if (pwmVal == 0) return BRAKE; 00098 else return BRAKE; 00099 } 00100 uint8_t Setpwm(int); 00101 uint8_t Setpwm(int pwmVal) 00102 { 00103 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; 00104 else return abs(pwmVal); 00105 } 00106 00107 int selectnum[16][4]= { 00108 //1,2,4,8 00109 {0,0,0,0}, 00110 {1,0,0,0}, 00111 {0,1,0,0}, 00112 {1,1,0,0}, 00113 {0,0,1,0}, 00114 {1,0,1,0}, 00115 {0,1,1,0}, 00116 {1,1,1,0}, 00117 {0,0,0,1}, 00118 {1,0,0,1}, 00119 {0,1,0,1}, 00120 {1,1,0,1}, 00121 {0,0,1,1}, 00122 {1,0,1,1}, 00123 {0,1,1,1}, 00124 {1,1,1,1} 00125 }; 00126 00127 DigitalIn EMC(PB_0); 00128 00129 void LedMode(int num) 00130 { 00131 POTENTIOMETER::dio[0]= selectnum[num][0]; 00132 POTENTIOMETER::dio[1]= selectnum[num][1];; 00133 POTENTIOMETER::dio[2]= selectnum[num][2];; 00134 POTENTIOMETER::dio[3]= selectnum[num][3];; 00135 00136 } 00137 00138 int Twsh; 00139 bool UP_flag = false; 00140 bool SW_flag = false; 00141 bool Air_flag = false; 00142 bool zyouge; 00143 bool zyougedo; 00144 bool dz1=true; 00145 bool dz1i=false; 00146 bool dz2=true; 00147 bool dz2i=false; 00148 bool dz3=true; 00149 bool dz3i=false; 00150 bool dz4=true; 00151 bool dz4i=false; 00152 bool zone; 00153 bool through=false; 00154 bool counts=false; 00155 bool mtc=false; 00156 bool mtc2 = false; 00157 bool emcs=false; 00158 bool kiri; 00159 00160 bool start_flag = true; 00161 00162 int mode =0; 00163 int cross=0;//十字数える用 00164 int cros=0; 00165 int count=100000;//wait代替え 00166 00167 ///*********PWM調整用*********/// 00168 int fast =60; 00169 int normal = 40; 00170 int slow = 20; 00171 00172 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00173 00174 #ifdef USE_SUBPROCESS 00175 #if USE_PROCESS_NUM>0 00176 static void Process0(void); 00177 #endif 00178 #if USE_PROCESS_NUM>1 00179 static void Process1(void); 00180 #endif 00181 #if USE_PROCESS_NUM>2 00182 static void Process2(void); 00183 #endif 00184 #if USE_PROCESS_NUM>3 00185 static void Process3(void); 00186 #endif 00187 #if USE_PROCESS_NUM>4 00188 static void Process4(void); 00189 #endif 00190 #if USE_PROCESS_NUM>5 00191 static void Process5(void); 00192 #endif 00193 #if USE_PROCESS_NUM>6 00194 static void Process6(void); 00195 #endif 00196 #if USE_PROCESS_NUM>7 00197 static void Process7(void); 00198 #endif 00199 #if USE_PROCESS_NUM>8 00200 static void Process8(void); 00201 #endif 00202 #if USE_PROCESS_NUM>9 00203 static void Process9(void); 00204 #endif 00205 #endif 00206 00207 void SystemProcessInitialize() 00208 { 00209 #pragma region USER-DEFINED_VARIABLE_INIT 00210 /*Replace here with the initialization code of your variables.*/ 00211 #pragma endregion USER-DEFINED_VARIABLE_INIT 00212 00213 lock = true; 00214 processChangeComp = true; 00215 current = DEFAULT_PROCESS; 00216 00217 #ifdef USE_SUBPROCESS 00218 #if USE_PROCESS_NUM>0 00219 Process[0] = Process0; 00220 #endif 00221 #if USE_PROCESS_NUM>1 00222 Process[1] = Process1; 00223 #endif 00224 #if USE_PROCESS_NUM>2 00225 Process[2] = Process2; 00226 #endif 00227 #if USE_PROCESS_NUM>3 00228 Process[3] = Process3; 00229 #endif 00230 #if USE_PROCESS_NUM>4 00231 Process[4] = Process4; 00232 #endif 00233 #if USE_PROCESS_NUM>5 00234 Process[5] = Process5; 00235 #endif 00236 #if USE_PROCESS_NUM>6 00237 Process[6] = Process6; 00238 #endif 00239 #if USE_PROCESS_NUM>7 00240 Process[7] = Process7; 00241 #endif 00242 #if USE_PROCESS_NUM>8 00243 Process[8] = Process8; 00244 #endif 00245 #if USE_PROCESS_NUM>9 00246 Process[9] = Process9; 00247 #endif 00248 #endif 00249 } 00250 00251 static void SystemProcessUpdate() 00252 { 00253 #ifdef USE_SUBPROCESS 00254 if(controller->Button.HOME) lock = false; 00255 00256 if(controller->Button.START && processChangeComp) { 00257 current++; 00258 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00259 processChangeComp = false; 00260 } else if(controller->Button.SELECT && processChangeComp) { 00261 current--; 00262 if (current < 0) current = 0; 00263 processChangeComp = false; 00264 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00265 #endif 00266 00267 #ifdef USE_MOTOR 00268 ACTUATORHUB::MOTOR::Motor::Update(motor); 00269 #endif 00270 00271 #ifdef USE_SOLENOID 00272 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00273 #endif 00274 00275 #ifdef USE_RS485 00276 ACTUATORHUB::ActuatorHub::Update(); 00277 #endif 00278 00279 } 00280 00281 int g[8]; 00282 00283 void SystemProcess() 00284 { 00285 SystemProcessInitialize(); 00286 00287 while(1) { 00288 if(EMC==0) { 00289 fast=40; 00290 normal=30; 00291 slow=20; 00292 cross=0;//十字数える用 00293 cros=0; 00294 count=0;//wait代替え 00295 UP_flag = false; 00296 SW_flag = false; 00297 Air_flag = false; 00298 zyouge=false; 00299 zyougedo=false; 00300 dz1=true; 00301 dz1i=false; 00302 dz2=true; 00303 dz2i=false; 00304 dz3=true; 00305 dz3i=false; 00306 dz4=true; 00307 dz4i=false; 00308 through=false; 00309 counts=false; 00310 mtc=false; 00311 mtc2 = false; 00312 current = 0; 00313 SystemProcessInitialize(); 00314 if(emcs==false) { 00315 AllActuatorReset(); 00316 emcs=true; 00317 } 00318 } else { 00319 emcs=false; 00320 } 00321 00322 if(mode==0) { 00323 if(ZoneSW) { 00324 LedMode(1); 00325 } else { 00326 LedMode(2); 00327 } 00328 } 00329 for(int i = 0; i < 8; i++) { 00330 g[i] = LineHub::GetPara(i); 00331 } 00332 if(ThSW) { 00333 Twsh=1; 00334 } else { 00335 Twsh=2; 00336 } 00337 00338 if(StertSW && start_flag == true) { 00339 start_flag = false; 00340 lock = false; 00341 if(ZoneSW) { 00342 count=0; 00343 mode=1; 00344 current = 6; 00345 00346 } else { 00347 count=0; 00348 mode=1; 00349 current = 4; 00350 } 00351 } 00352 if(DWSW||DOSW) { 00353 if(DWSW) { 00354 zyouge=true; 00355 } 00356 if(DOSW) { 00357 if(DOLS) { 00358 motor[MOTOR_0].pwm = 100; 00359 motor[MOTOR_0].dir = BRAKE; 00360 } else { 00361 motor[MOTOR_0].pwm = 50; 00362 motor[MOTOR_0].dir = BACK; 00363 } 00364 } 00365 } else if(zyouge==false) { 00366 motor[MOTOR_0].pwm = 100; 00367 motor[MOTOR_0].dir = BRAKE; 00368 } 00369 if(zyouge) { 00370 if(Twsh==1) { 00371 motor[MOTOR_0].pwm = 215; 00372 motor[MOTOR_0].dir = FOR; 00373 if(UPLS) { 00374 motor[MOTOR_0].pwm = 100; 00375 motor[MOTOR_0].dir = BRAKE; 00376 zyouge=false; 00377 } 00378 } 00379 if(Twsh==2) { 00380 motor[MOTOR_0].pwm = 245; 00381 motor[MOTOR_0].dir = FOR; 00382 if(UPLS2) { 00383 motor[MOTOR_0].pwm = 100; 00384 motor[MOTOR_0].dir = BRAKE; 00385 zyouge=false; 00386 } 00387 } 00388 } 00389 if(AIRSW) { 00390 if(SW_flag==false) { 00391 if(Air_flag==false) { 00392 solenoid.solenoid3 = SOLENOID_ON; 00393 Air_flag=true; 00394 SW_flag=true; 00395 } else { 00396 solenoid.solenoid3 = SOLENOID_OFF; 00397 Air_flag=false; 00398 SW_flag=true; 00399 } 00400 } 00401 } else { 00402 if( SW_flag==true) { 00403 count++; 00404 if(count>=2000) { 00405 SW_flag=false; 00406 count=0; 00407 } 00408 } 00409 } 00410 00411 //printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]); 00412 00413 #ifdef USE_MU 00414 controller = CONTROLLER::Controller::GetData(); 00415 #endif 00416 00417 #ifdef USE_ERRORCHECK 00418 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { //& start_flag 00419 CONTROLLER::Controller::DataReset(); 00420 AllActuatorReset(); 00421 lock = true; 00422 } else 00423 #endif 00424 { 00425 00426 #ifdef USE_SUBPROCESS 00427 if(!lock) { 00428 Process[current](); 00429 } else 00430 #endif 00431 { 00432 //ロック時の処理 00433 } 00434 } 00435 00436 SystemProcessUpdate(); 00437 } 00438 } 00439 00440 #pragma region PROCESS 00441 #ifdef USE_SUBPROCESS 00442 #if USE_PROCESS_NUM>0 00443 static void Process0() 00444 { 00445 /* 00446 mode=1; 00447 LedMode(1); 00448 if(ThSW) { 00449 Twsh=1; 00450 } else { 00451 Twsh=2; 00452 } 00453 00454 if(StertSW) { 00455 //start_flag == false; 00456 //lock = false; 00457 if(ZoneSW) { 00458 mode=1; 00459 current = 2; 00460 } else { 00461 mode=1; 00462 current = 5; 00463 } 00464 } 00465 if(DWSW) { 00466 zyouge=true; 00467 } 00468 if(DOSW) { 00469 zyougedo=true; 00470 } 00471 if(AIRSW) { 00472 if(SW_flag==false) { 00473 if(Air_flag==false) { 00474 solenoid.solenoid3 = SOLENOID_ON; 00475 Air_flag=true; 00476 SW_flag=true; 00477 } else { 00478 solenoid.solenoid3 = SOLENOID_OFF; 00479 Air_flag=false; 00480 SW_flag=true; 00481 } 00482 } 00483 } else { 00484 SW_flag=false; 00485 } 00486 00487 if(zyouge) { 00488 if(Twsh==1) { 00489 motor[MOTOR_0].pwm = 220; 00490 motor[MOTOR_0].dir = FOR; 00491 if(UPLS) { 00492 motor[MOTOR_0].pwm = 100; 00493 motor[MOTOR_0].dir = BRAKE; 00494 zyouge=false; 00495 } 00496 } 00497 if(Twsh==2) { 00498 motor[MOTOR_0].pwm = 230; 00499 motor[MOTOR_0].dir = FOR; 00500 if(UPLS2) { 00501 motor[MOTOR_0].pwm = 100; 00502 motor[MOTOR_0].dir = BRAKE; 00503 zyouge=false; 00504 } 00505 } 00506 } 00507 if(zyougedo) { 00508 motor[MOTOR_0].pwm = 50; 00509 motor[MOTOR_0].dir = BACK; 00510 if(DOLS) { 00511 motor[MOTOR_0].pwm = 100; 00512 motor[MOTOR_0].dir = BRAKE; 00513 zyougedo=false; 00514 } 00515 } 00516 */ 00517 } 00518 #endif 00519 00520 #if USE_PROCESS_NUM>1 00521 static void Process1() 00522 { 00523 LedMode(6); 00524 start_flag = true; 00525 mtc=false; 00526 mtc2 = false; 00527 mode=1; 00528 cross=0; 00529 Twsh=1; 00530 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00531 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00532 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00533 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00534 00535 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00536 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00537 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00538 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00539 00540 if(controller->Button.UP||controller->Button.DOWN) { 00541 00542 if(controller->Button.UP) { 00543 motor[MOTOR_0].pwm =200; 00544 motor[MOTOR_0].dir = FOR; 00545 if(UPLS) { 00546 motor[MOTOR_0].pwm = 0; 00547 motor[MOTOR_0].dir = BRAKE; 00548 } 00549 } 00550 if(controller->Button.DOWN) { 00551 motor[MOTOR_0].pwm = 50; 00552 motor[MOTOR_0].dir = BACK; 00553 } 00554 } else { 00555 motor[MOTOR_0].pwm = 0; 00556 motor[MOTOR_0].dir = BRAKE; 00557 } 00558 00559 if(controller->Button.Y) { 00560 if(dz2==true) { 00561 if(dz2i==false) { 00562 solenoid.solenoid2 = SOLENOID_ON; 00563 dz2i=true; 00564 } else { 00565 solenoid.solenoid2 = SOLENOID_OFF; 00566 dz2i=false; 00567 } 00568 dz2=false; 00569 } 00570 } else { 00571 dz2=true; 00572 } 00573 00574 if(controller->Button.A) { 00575 if(dz3==true) { 00576 if(dz3i==false) { 00577 00578 dz3i=true; 00579 } else { 00580 solenoid.solenoid3 = SOLENOID_OFF; 00581 dz3i=false; 00582 } 00583 dz3=false; 00584 } 00585 } else { 00586 dz3=true; 00587 } 00588 00589 if(controller->Button.B) { 00590 if(dz4==true) { 00591 if(dz4i==false) { 00592 solenoid.solenoid4 = SOLENOID_ON; 00593 dz4i=true; 00594 } else { 00595 solenoid.solenoid4 = SOLENOID_OFF; 00596 dz4i=false; 00597 } 00598 dz4=false; 00599 } 00600 } else { 00601 dz4=true; 00602 } 00603 /* 00604 if(controller->Button.RIGHT){ 00605 motor[MOTOR_1].dir = FOR; 00606 motor[MOTOR_1].pwm = 60; 00607 if (ARM_1){ 00608 motor[MOTOR_1].dir = BRAKE; 00609 } 00610 } 00611 else if(controller->Button.LEFT){ 00612 motor[MOTOR_1].dir = BACK; 00613 motor[MOTOR_1].pwm = 60; 00614 if (ARM_0){ 00615 motor[MOTOR_1].dir = BRAKE; 00616 } 00617 } 00618 */ 00619 } 00620 #endif 00621 00622 #if USE_PROCESS_NUM>2 00623 static void Process2() 00624 { 00625 // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]); 00626 LedMode(3); 00627 count++; 00628 if(mode==0||mode==1) { 00629 00630 if(Twsh==2) { 00631 solenoid.solenoid2 = SOLENOID_ON; 00632 } 00633 if(counts==false&&g[0]==0) { 00634 cros++; 00635 counts=true; 00636 } 00637 if(counts==true&&g[0]==99) { 00638 counts=false; 00639 } 00640 00641 if(cros==2) { 00642 00643 // mode=21; 00644 // count=0; 00645 // cros=0; 00646 }//ゆっくりモードに入れなかった時の保険 00647 if(g[0]==0) { 00648 fast=35; 00649 normal=20; 00650 slow=15; 00651 } 00652 00653 switch(g[2]) { 00654 case 98: 00655 motor[TIRE_FR].pwm = normal; 00656 motor[TIRE_FR].dir = FOR; 00657 motor[TIRE_FL].pwm = normal; 00658 motor[TIRE_FL].dir = FOR; 00659 motor[TIRE_BR].pwm = normal; 00660 motor[TIRE_BR].dir = BACK; 00661 motor[TIRE_BL].pwm = normal; 00662 motor[TIRE_BL].dir = BACK; 00663 if(count>30000) { 00664 cross++; 00665 count=0; 00666 } 00667 mtc2=true; 00668 if(cross==2) { 00669 mode=22; 00670 count=0; 00671 cross=0; 00672 fast=40; 00673 normal=30; 00674 slow=20; 00675 } 00676 break; 00677 case 0: 00678 motor[TIRE_FR].pwm = fast; 00679 motor[TIRE_FR].dir = FOR; 00680 motor[TIRE_FL].pwm = fast; 00681 motor[TIRE_FL].dir = FOR; 00682 motor[TIRE_BR].pwm = fast; 00683 motor[TIRE_BR].dir = BACK; 00684 motor[TIRE_BL].pwm = fast; 00685 motor[TIRE_BL].dir = BACK; 00686 mtc2=true; 00687 break; 00688 case 255: 00689 motor[TIRE_FR].pwm = normal; 00690 motor[TIRE_FR].dir = FOR; 00691 motor[TIRE_FL].pwm = fast; 00692 motor[TIRE_FL].dir = FOR; 00693 motor[TIRE_BR].pwm = fast; 00694 motor[TIRE_BR].dir = BACK; 00695 motor[TIRE_BL].pwm = normal; 00696 motor[TIRE_BL].dir = BACK; 00697 mtc2=true; 00698 break; 00699 case 253: 00700 motor[TIRE_FR].pwm = 0; 00701 motor[TIRE_FR].dir = FREE; 00702 motor[TIRE_FL].pwm = slow; 00703 motor[TIRE_FL].dir = FOR; 00704 motor[TIRE_BR].pwm = slow; 00705 motor[TIRE_BR].dir = BACK; 00706 motor[TIRE_BL].pwm = 0; 00707 motor[TIRE_BL].dir = FREE; 00708 mtc2=true; 00709 break; 00710 case 254: 00711 motor[TIRE_FR].pwm = 0; 00712 motor[TIRE_FR].dir = FREE; 00713 motor[TIRE_FL].pwm = normal; 00714 motor[TIRE_FL].dir = FOR; 00715 motor[TIRE_BR].pwm = normal; 00716 motor[TIRE_BR].dir = BACK; 00717 motor[TIRE_BL].pwm = 0; 00718 motor[TIRE_BL].dir = FREE; 00719 mtc2=true; 00720 break; 00721 case 1: 00722 motor[TIRE_FR].pwm = fast; 00723 motor[TIRE_FR].dir = FOR; 00724 motor[TIRE_FL].pwm = slow; 00725 motor[TIRE_FL].dir = FOR; 00726 motor[TIRE_BR].pwm = slow; 00727 motor[TIRE_BR].dir = BACK; 00728 motor[TIRE_BL].pwm = fast; 00729 motor[TIRE_BL].dir = BACK; 00730 mtc2=true; 00731 break; 00732 case 3: 00733 motor[TIRE_FR].pwm = slow; 00734 motor[TIRE_FR].dir = FOR; 00735 motor[TIRE_FL].pwm = 0; 00736 motor[TIRE_FL].dir = FREE; 00737 motor[TIRE_BR].pwm = 0; 00738 motor[TIRE_BR].dir = FREE; 00739 motor[TIRE_BL].pwm = slow; 00740 motor[TIRE_BL].dir = BACK; 00741 mtc2=true; 00742 break; 00743 case 2: 00744 motor[TIRE_FR].pwm = normal; 00745 motor[TIRE_FR].dir = FOR; 00746 motor[TIRE_FL].pwm = 0; 00747 motor[TIRE_FL].dir = FREE; 00748 motor[TIRE_BR].pwm = 0; 00749 motor[TIRE_BR].dir = FREE; 00750 motor[TIRE_BL].pwm = normal; 00751 motor[TIRE_BL].dir = BACK; 00752 mtc2=true; 00753 break; 00754 } 00755 if(g[2]!=98&&mtc2==true) { 00756 switch(g[3]) { 00757 case 0: 00758 mtc2=false; 00759 break; 00760 case 255: 00761 motor[TIRE_BL].pwm += 5; 00762 mtc2=false; 00763 break; 00764 case 253: 00765 motor[TIRE_BL].pwm += 10; 00766 mtc2=false; 00767 break; 00768 case 254: 00769 motor[TIRE_BL].pwm += 20; 00770 motor[TIRE_FL].pwm = 0; 00771 mtc2=false; 00772 break; 00773 case 1: 00774 motor[TIRE_FL].pwm += 5; 00775 mtc2=false; 00776 break; 00777 case 3: 00778 motor[TIRE_FL].pwm += 10; 00779 mtc2=false; 00780 break; 00781 case 2: 00782 motor[TIRE_FL].pwm += 20; 00783 motor[TIRE_BL].pwm = 0; 00784 mtc2=false; 00785 break; 00786 } 00787 } 00788 } else if(mode==21) { 00789 motor[TIRE_FR].pwm = 30; 00790 motor[TIRE_FR].dir = FOR; 00791 motor[TIRE_FL].pwm = 30; 00792 motor[TIRE_FL].dir = FOR; 00793 motor[TIRE_BR].pwm = 30; 00794 motor[TIRE_BR].dir = BACK; 00795 motor[TIRE_BL].pwm = 30; 00796 motor[TIRE_BL].dir = BACK; 00797 if(g[2]==98&&count>=10000) { 00798 mode=22; 00799 count=0; 00800 } 00801 } 00802 if(YELLOW==true) { 00803 if(mode==22) { 00804 motor[TIRE_FR].pwm = 100; 00805 motor[TIRE_FR].dir = BRAKE; 00806 motor[TIRE_FL].pwm = 100; 00807 motor[TIRE_FL].dir = BRAKE; 00808 motor[TIRE_BR].pwm = 100; 00809 motor[TIRE_BR].dir = BRAKE; 00810 motor[TIRE_BL].pwm = 100; 00811 motor[TIRE_BL].dir = BRAKE; 00812 if(count>=10000) { 00813 mode=23; 00814 } 00815 } else if(mode==23) { 00816 motor[TIRE_FR].pwm = 20; 00817 motor[TIRE_FR].dir = BACK; 00818 motor[TIRE_FL].pwm = 20; 00819 motor[TIRE_FL].dir = BACK; 00820 motor[TIRE_BR].pwm = 20; 00821 motor[TIRE_BR].dir = FOR; 00822 motor[TIRE_BL].pwm = 20; 00823 motor[TIRE_BL].dir = FOR; 00824 if(g[2]==98) { 00825 mode=24; 00826 count=0; 00827 } 00828 } else if(mode==24) { 00829 motor[TIRE_FR].pwm = 20; 00830 motor[TIRE_FR].dir = BACK; 00831 motor[TIRE_FL].pwm = 20; 00832 motor[TIRE_FL].dir = BACK; 00833 motor[TIRE_BR].pwm = 20; 00834 motor[TIRE_BR].dir = FOR; 00835 motor[TIRE_BL].pwm = 20; 00836 motor[TIRE_BL].dir = FOR; 00837 if(g[2]!=98) { 00838 count=0; 00839 mode=25; 00840 } 00841 } else if(mode==25) { 00842 motor[TIRE_FR].pwm = 10; 00843 motor[TIRE_FR].dir = BRAKE; 00844 motor[TIRE_FL].pwm = 10; 00845 motor[TIRE_FL].dir = BRAKE; 00846 motor[TIRE_BR].pwm = 10; 00847 motor[TIRE_BR].dir = BRAKE; 00848 motor[TIRE_BL].pwm = 10; 00849 motor[TIRE_BL].dir = BRAKE; 00850 if(count>=10000) { 00851 mode=26; 00852 count=0; 00853 } 00854 } else if(mode==26) { 00855 motor[TIRE_FR].pwm = 20; 00856 motor[TIRE_FR].dir = FOR; 00857 motor[TIRE_FL].pwm = 20; 00858 motor[TIRE_FL].dir = FOR; 00859 motor[TIRE_BR].pwm = 20; 00860 motor[TIRE_BR].dir = BACK; 00861 motor[TIRE_BL].pwm = 20; 00862 motor[TIRE_BL].dir = BACK; 00863 if(count>=5000) { 00864 mode=27; 00865 count=0; 00866 } 00867 } else if(mode==27) { 00868 motor[TIRE_FR].pwm = 20; 00869 motor[TIRE_FR].dir = FOR; 00870 motor[TIRE_FL].pwm = 20; 00871 motor[TIRE_FL].dir = FOR; 00872 motor[TIRE_BR].pwm = 20; 00873 motor[TIRE_BR].dir = FOR; 00874 motor[TIRE_BL].pwm = 20; 00875 motor[TIRE_BL].dir = FOR; 00876 //どれくらい回転するかって値 00877 if(count>=2500) { 00878 count=0; 00879 mode=30; 00880 } 00881 } else if(mode==24) { 00882 motor[TIRE_FR].pwm = 10; 00883 motor[TIRE_FR].dir = BRAKE; 00884 motor[TIRE_FL].pwm = 10; 00885 motor[TIRE_FL].dir = BRAKE; 00886 motor[TIRE_BR].pwm = 10; 00887 motor[TIRE_BR].dir = BRAKE; 00888 motor[TIRE_BL].pwm = 10; 00889 motor[TIRE_BL].dir = BRAKE; 00890 if(count>=10000) { 00891 mode=30; 00892 count=0; 00893 } 00894 } 00895 } else { 00896 if(mode==22) { 00897 motor[TIRE_FR].pwm = 0; 00898 motor[TIRE_FR].dir = FREE; 00899 motor[TIRE_FL].pwm = 25; 00900 motor[TIRE_FL].dir = FOR; 00901 motor[TIRE_BR].pwm = 25; 00902 motor[TIRE_BR].dir = BACK; 00903 motor[TIRE_BL].pwm = 0; 00904 motor[TIRE_BL].dir = FREE; 00905 if(g[0]==0) { 00906 mode=30; 00907 cros=0; 00908 } 00909 } 00910 } 00911 if(mode==30) { 00912 motor[TIRE_FR].pwm = 59; 00913 motor[TIRE_FR].dir = BACK; 00914 motor[TIRE_FL].pwm = 62; 00915 motor[TIRE_FL].dir = FOR; 00916 motor[TIRE_BR].pwm = 59; 00917 motor[TIRE_BR].dir = BACK; 00918 motor[TIRE_BL].pwm = 62; 00919 motor[TIRE_BL].dir = FOR; 00920 if(g[0]==98) { 00921 mode=31; 00922 count=0; 00923 } 00924 } else if(mode==31) { 00925 motor[TIRE_FR].pwm = 0; 00926 motor[TIRE_FR].dir = FREE; 00927 motor[TIRE_FL].pwm = 0; 00928 motor[TIRE_FL].dir = FREE; 00929 motor[TIRE_BR].pwm = 0; 00930 motor[TIRE_BR].dir = FREE; 00931 motor[TIRE_BL].pwm = 0; 00932 motor[TIRE_BL].dir = FREE; 00933 if(count>=5000) { 00934 count=0; 00935 mode=32; 00936 } 00937 } else if(mode==32) { 00938 motor[TIRE_FR].pwm = 10; 00939 motor[TIRE_FR].dir = BRAKE; 00940 motor[TIRE_FL].pwm = 10; 00941 motor[TIRE_FL].dir = BRAKE; 00942 motor[TIRE_BR].pwm = 10; 00943 motor[TIRE_BR].dir = BRAKE; 00944 motor[TIRE_BL].pwm = 10; 00945 motor[TIRE_BL].dir = BRAKE; 00946 if(count>=40000) { 00947 mode=33; 00948 count=0; 00949 } 00950 } else if(mode==33) { 00951 motor[TIRE_FR].pwm = 0; 00952 motor[TIRE_FR].dir = BACK; 00953 motor[TIRE_FL].pwm = 30; 00954 motor[TIRE_FL].dir = FOR; 00955 motor[TIRE_BR].pwm = 30; 00956 motor[TIRE_BR].dir = BACK; 00957 motor[TIRE_BL].pwm = 0; 00958 motor[TIRE_BL].dir = FOR; 00959 if(g[3]==0) { 00960 count=0; 00961 if(Twsh==2) { 00962 solenoid.solenoid2 = SOLENOID_OFF; 00963 } 00964 mode=40; 00965 } 00966 } else if(mode==40) { 00967 if(LIB) { 00968 mode=42; 00969 } 00970 if(counts==false&&g[0]==0) { 00971 cros++; 00972 counts=true; 00973 } 00974 if(counts==true&&g[0]==99) { 00975 counts=false; 00976 } 00977 00978 if(cros==1) { 00979 // mode=41; 00980 //count=0; 00981 //cros=0; 00982 }//ゆっくりモードに入れなかった時の保険 00983 00984 switch(g[3]) { 00985 case 98: 00986 motor[TIRE_FR].pwm = normal; 00987 motor[TIRE_FR].dir = BACK; 00988 motor[TIRE_FL].pwm = normal; 00989 motor[TIRE_FL].dir = BACK; 00990 motor[TIRE_BR].pwm = normal; 00991 motor[TIRE_BR].dir = FOR; 00992 motor[TIRE_BL].pwm = normal; 00993 motor[TIRE_BL].dir = FOR; 00994 if(count>20000) { 00995 cross++; 00996 count=0; 00997 } 00998 mtc2=true; 00999 if(cross==2) { 01000 mode=41; 01001 count=0; 01002 cross=0; 01003 } 01004 break; 01005 case 0: 01006 motor[TIRE_FR].pwm = fast; 01007 motor[TIRE_FR].dir = BACK; 01008 motor[TIRE_FL].pwm = fast; 01009 motor[TIRE_FL].dir = BACK; 01010 motor[TIRE_BR].pwm = fast; 01011 motor[TIRE_BR].dir = FOR; 01012 motor[TIRE_BL].pwm = fast; 01013 motor[TIRE_BL].dir = FOR; 01014 mtc2=true; 01015 break; 01016 case 255: 01017 motor[TIRE_FR].pwm = slow; 01018 motor[TIRE_FR].dir = BACK; 01019 motor[TIRE_FL].pwm = fast; 01020 motor[TIRE_FL].dir = BACK; 01021 motor[TIRE_BR].pwm = fast; 01022 motor[TIRE_BR].dir = FOR; 01023 motor[TIRE_BL].pwm = slow; 01024 motor[TIRE_BL].dir = FOR; 01025 mtc2=true; 01026 break; 01027 case 253: 01028 motor[TIRE_FR].pwm = 0; 01029 motor[TIRE_FR].dir = FREE; 01030 motor[TIRE_FL].pwm = slow; 01031 motor[TIRE_FL].dir = BACK; 01032 motor[TIRE_BR].pwm = slow; 01033 motor[TIRE_BR].dir = FOR; 01034 motor[TIRE_BL].pwm = 0; 01035 motor[TIRE_BL].dir = FREE; 01036 mtc2=true; 01037 break; 01038 case 254: 01039 motor[TIRE_FR].pwm = 0; 01040 motor[TIRE_FR].dir = FREE; 01041 motor[TIRE_FL].pwm = normal; 01042 motor[TIRE_FL].dir = BACK; 01043 motor[TIRE_BR].pwm = normal; 01044 motor[TIRE_BR].dir = FOR; 01045 motor[TIRE_BL].pwm = 0; 01046 motor[TIRE_BL].dir = FREE; 01047 mtc2=true; 01048 break; 01049 case 1: 01050 motor[TIRE_FR].pwm = fast; 01051 motor[TIRE_FR].dir = BACK; 01052 motor[TIRE_FL].pwm = slow; 01053 motor[TIRE_FL].dir = BACK; 01054 motor[TIRE_BR].pwm = slow; 01055 motor[TIRE_BR].dir = FOR; 01056 motor[TIRE_BL].pwm = fast; 01057 motor[TIRE_BL].dir = FOR; 01058 mtc2=true; 01059 break; 01060 case 3: 01061 motor[TIRE_FR].pwm = slow; 01062 motor[TIRE_FR].dir = BACK; 01063 motor[TIRE_FL].pwm = 0; 01064 motor[TIRE_FL].dir = FREE; 01065 motor[TIRE_BR].pwm = 0; 01066 motor[TIRE_BR].dir = FREE; 01067 motor[TIRE_BL].pwm = slow; 01068 motor[TIRE_BL].dir = FOR; 01069 mtc2=true; 01070 break; 01071 case 2: 01072 motor[TIRE_FR].pwm = normal; 01073 motor[TIRE_FR].dir = BACK; 01074 motor[TIRE_FL].pwm = 0; 01075 motor[TIRE_FL].dir = FREE; 01076 motor[TIRE_BR].pwm = 0; 01077 motor[TIRE_BR].dir = FREE; 01078 motor[TIRE_BL].pwm = normal; 01079 motor[TIRE_BL].dir = FOR; 01080 mtc2=true; 01081 break; 01082 } 01083 if(g[2]!=98&&mtc2==true) { 01084 switch(g[2]) { 01085 case 0: 01086 mtc2=false; 01087 break; 01088 case 255: 01089 motor[TIRE_FL].pwm += 5; 01090 mtc2=false; 01091 break; 01092 case 253: 01093 motor[TIRE_FL].pwm += 10; 01094 mtc2=false; 01095 break; 01096 case 254: 01097 motor[TIRE_FL].pwm += 20; 01098 motor[TIRE_BL].pwm = 0; 01099 mtc2=false; 01100 break; 01101 case 1: 01102 motor[TIRE_BL].pwm += 5; 01103 mtc2=false; 01104 break; 01105 case 3: 01106 motor[TIRE_BL].pwm += 10; 01107 mtc2=false; 01108 break; 01109 case 2: 01110 motor[TIRE_BL].pwm += 20; 01111 motor[TIRE_FL].pwm = 0; 01112 mtc2=false; 01113 break; 01114 } 01115 } 01116 } else if(mode==41) { 01117 if(LIF||LIB) { 01118 mode=42; 01119 } 01120 motor[TIRE_FR].pwm = 0; 01121 motor[TIRE_FR].dir = FREE; 01122 motor[TIRE_FL].pwm = 30; 01123 motor[TIRE_FL].dir = BACK; 01124 motor[TIRE_BR].pwm = 30; 01125 motor[TIRE_BR].dir = FOR; 01126 motor[TIRE_BL].pwm = 0; 01127 motor[TIRE_BL].dir = FREE; 01128 if(g[1]==0) { 01129 count=0; 01130 mode=70; 01131 } 01132 } else if(mode==42) { 01133 motor[TIRE_FR].pwm = 20; 01134 motor[TIRE_FR].dir = FOR; 01135 motor[TIRE_FL].pwm = 0; 01136 motor[TIRE_FL].dir = FREE; 01137 motor[TIRE_BR].pwm = 0; 01138 motor[TIRE_BR].dir = FREE; 01139 motor[TIRE_BL].pwm = 20; 01140 motor[TIRE_BL].dir = BACK; 01141 if(g[1]==0) { 01142 count=0; 01143 mode=70; 01144 } 01145 } else if(mode==70) { 01146 switch(g[1]) { 01147 case 99: 01148 if(count>=100000) { 01149 mode=100; 01150 } 01151 break; 01152 case 98: 01153 motor[TIRE_FR].pwm = normal; 01154 motor[TIRE_FR].dir = FOR; 01155 motor[TIRE_FL].pwm = normal; 01156 motor[TIRE_FL].dir = BACK; 01157 motor[TIRE_BR].pwm = normal; 01158 motor[TIRE_BR].dir = FOR; 01159 motor[TIRE_BL].pwm = normal; 01160 motor[TIRE_BL].dir = BACK; 01161 break; 01162 case 0: 01163 motor[TIRE_FR].pwm = 80; 01164 motor[TIRE_FR].dir = FOR; 01165 motor[TIRE_FL].pwm = 80; 01166 motor[TIRE_FL].dir = BACK; 01167 motor[TIRE_BR].pwm = 80; 01168 motor[TIRE_BR].dir = FOR; 01169 motor[TIRE_BL].pwm = 80; 01170 motor[TIRE_BL].dir = BACK; 01171 mtc=true; 01172 break; 01173 case 255: 01174 motor[TIRE_FR].pwm = fast; 01175 motor[TIRE_FR].dir = FOR; 01176 motor[TIRE_FL].pwm = slow; 01177 motor[TIRE_FL].dir = BACK; 01178 motor[TIRE_BR].pwm = slow; 01179 motor[TIRE_BR].dir = FOR; 01180 motor[TIRE_BL].pwm = fast; 01181 motor[TIRE_BL].dir = BACK; 01182 mtc=true; 01183 break; 01184 case 253: 01185 motor[TIRE_FR].pwm = slow; 01186 motor[TIRE_FR].dir = FOR; 01187 motor[TIRE_FL].pwm = 0; 01188 motor[TIRE_FL].dir = FREE; 01189 motor[TIRE_BR].pwm = 0; 01190 motor[TIRE_BR].dir = FREE; 01191 motor[TIRE_BL].pwm = slow; 01192 motor[TIRE_BL].dir = BACK; 01193 mtc=true; 01194 break; 01195 case 254: 01196 motor[TIRE_FR].pwm = normal; 01197 motor[TIRE_FR].dir = FOR; 01198 motor[TIRE_FL].pwm = 0; 01199 motor[TIRE_FL].dir = FREE; 01200 motor[TIRE_BR].pwm = 0; 01201 motor[TIRE_BR].dir = FREE; 01202 motor[TIRE_BL].pwm = normal; 01203 motor[TIRE_BL].dir = BACK; 01204 mtc=true; 01205 break; 01206 case 1: 01207 motor[TIRE_FR].pwm = slow; 01208 motor[TIRE_FR].dir = FOR; 01209 motor[TIRE_FL].pwm = fast; 01210 motor[TIRE_FL].dir = BACK; 01211 motor[TIRE_BR].pwm = fast; 01212 motor[TIRE_BR].dir = FOR; 01213 motor[TIRE_BL].pwm = slow; 01214 motor[TIRE_BL].dir = BACK; 01215 mtc=true; 01216 break; 01217 case 3: 01218 motor[TIRE_FR].pwm = 0; 01219 motor[TIRE_FR].dir = FREE; 01220 motor[TIRE_FL].pwm = slow; 01221 motor[TIRE_FL].dir = BACK; 01222 motor[TIRE_BR].pwm = slow; 01223 motor[TIRE_BR].dir = FOR; 01224 motor[TIRE_BL].pwm = 0; 01225 motor[TIRE_BL].dir = FREE; 01226 mtc=true; 01227 break; 01228 case 2: 01229 motor[TIRE_FR].pwm = 0; 01230 motor[TIRE_FR].dir = FREE; 01231 motor[TIRE_FL].pwm = normal; 01232 motor[TIRE_FL].dir = BACK; 01233 motor[TIRE_BR].pwm = normal; 01234 motor[TIRE_BR].dir = FOR; 01235 motor[TIRE_BL].pwm = 0; 01236 motor[TIRE_BL].dir = FREE; 01237 mtc=true; 01238 break; 01239 } 01240 if(g[1]!=98&&mtc==true) { 01241 switch(g[0]) { 01242 case 0: 01243 mtc=false; 01244 break; 01245 case 255: 01246 motor[TIRE_FL].pwm += 5; 01247 mtc=false; 01248 break; 01249 case 253: 01250 motor[TIRE_FL].pwm += 10; 01251 mtc=false; 01252 break; 01253 case 254: 01254 motor[TIRE_FL].pwm += 20; 01255 motor[TIRE_FR].pwm = 0; 01256 mtc=false; 01257 break; 01258 case 1: 01259 motor[TIRE_FR].pwm += 5; 01260 mtc=false; 01261 break; 01262 case 3: 01263 motor[TIRE_FR].pwm += 10; 01264 mtc=false; 01265 break; 01266 case 2: 01267 motor[TIRE_FR].pwm += 20; 01268 motor[TIRE_FL].pwm = 0; 01269 mtc=false; 01270 break; 01271 } 01272 } 01273 } else if(mode==100) { 01274 motor[TIRE_FR].pwm = 20; 01275 motor[TIRE_FR].dir = FOR; 01276 motor[TIRE_FL].pwm = 20; 01277 motor[TIRE_FL].dir = BACK; 01278 motor[TIRE_BR].pwm = 23; 01279 motor[TIRE_BR].dir = FOR; 01280 motor[TIRE_BL].pwm = 20; 01281 motor[TIRE_BL].dir = BACK; 01282 } else { 01283 } 01284 } 01285 #endif 01286 01287 #if USE_PROCESS_NUM>3 01288 static void Process3() 01289 { 01290 } 01291 #endif 01292 01293 #if USE_PROCESS_NUM>4 01294 static void Process4() 01295 { 01296 LedMode(4); 01297 count++; 01298 if(mode==1) {//スタートゾーンから白線検知 01299 fast=40; 01300 normal=30; 01301 slow=20; 01302 motor[TIRE_FR].pwm = 27; 01303 motor[TIRE_FR].dir = FOR; 01304 motor[TIRE_FL].pwm = 33; 01305 motor[TIRE_FL].dir = BACK; 01306 motor[TIRE_BR].pwm = 33; 01307 motor[TIRE_BR].dir = FOR; 01308 motor[TIRE_BL].pwm = 27; 01309 motor[TIRE_BL].dir = BACK; 01310 //スタート後どれくらい無検知でうごくか↓ 01311 if(g[2]==0&&count>=100000) { 01312 count=100000; 01313 cross=0; 01314 mode=3; 01315 } 01316 } else if(mode==3) { //横ライントレースから縦ライントレースへ 01317 motor[TIRE_FR].pwm = 30; 01318 motor[TIRE_FR].dir = FOR; 01319 motor[TIRE_FL].pwm = 0; 01320 motor[TIRE_FL].dir = FREE; 01321 motor[TIRE_BR].pwm = 0; 01322 motor[TIRE_BR].dir = FREE; 01323 motor[TIRE_BL].pwm = 30; 01324 motor[TIRE_BL].dir = BACK; 01325 if(g[1]==0) { 01326 mode=10; 01327 count=100000; 01328 } 01329 } else if(mode==10) { //縦ライントレース 01330 01331 if(counts==false&&g[2]==0) { 01332 cros++; 01333 counts=true; 01334 } 01335 if(counts==true&&g[2]==99) { 01336 counts=false; 01337 } 01338 01339 if(cros==Twsh) { 01340 mode=11; 01341 count=0; 01342 cros=0; 01343 }//ゆっくりモードに入れなかった時の保険 01344 switch(g[1]) { 01345 case 98: 01346 motor[TIRE_FR].pwm = normal; 01347 motor[TIRE_FR].dir = FOR; 01348 motor[TIRE_FL].pwm = normal; 01349 motor[TIRE_FL].dir = BACK; 01350 motor[TIRE_BR].pwm = normal; 01351 motor[TIRE_BR].dir = FOR; 01352 motor[TIRE_BL].pwm = normal; 01353 motor[TIRE_BL].dir = BACK; 01354 mtc=true; 01355 if(count>100000) { 01356 cross++; 01357 count=0; 01358 } 01359 if(cross==Twsh) { 01360 mode=11; 01361 count=0; 01362 cross=0; 01363 } 01364 break; 01365 case 0: 01366 motor[TIRE_FR].pwm = fast; 01367 motor[TIRE_FR].dir = FOR; 01368 motor[TIRE_FL].pwm = fast; 01369 motor[TIRE_FL].dir = BACK; 01370 motor[TIRE_BR].pwm = fast; 01371 motor[TIRE_BR].dir = FOR; 01372 motor[TIRE_BL].pwm = fast; 01373 motor[TIRE_BL].dir = BACK; 01374 mtc=true; 01375 break; 01376 case 255: 01377 motor[TIRE_FR].pwm = fast; 01378 motor[TIRE_FR].dir = FOR; 01379 motor[TIRE_FL].pwm = slow; 01380 motor[TIRE_FL].dir = BACK; 01381 motor[TIRE_BR].pwm = slow; 01382 motor[TIRE_BR].dir = FOR; 01383 motor[TIRE_BL].pwm = fast; 01384 motor[TIRE_BL].dir = BACK; 01385 mtc=true; 01386 break; 01387 case 253: 01388 motor[TIRE_FR].pwm = slow; 01389 motor[TIRE_FR].dir = FOR; 01390 motor[TIRE_FL].pwm = 0; 01391 motor[TIRE_FL].dir = FREE; 01392 motor[TIRE_BR].pwm = 0; 01393 motor[TIRE_BR].dir = FREE; 01394 motor[TIRE_BL].pwm = slow; 01395 motor[TIRE_BL].dir = BACK; 01396 mtc=true; 01397 break; 01398 case 254: 01399 motor[TIRE_FR].pwm = normal; 01400 motor[TIRE_FR].dir = FOR; 01401 motor[TIRE_FL].pwm = 0; 01402 motor[TIRE_FL].dir = FREE; 01403 motor[TIRE_BR].pwm = 0; 01404 motor[TIRE_BR].dir = FREE; 01405 motor[TIRE_BL].pwm = normal; 01406 motor[TIRE_BL].dir = BACK; 01407 mtc=true; 01408 break; 01409 case 1: 01410 motor[TIRE_FR].pwm = slow; 01411 motor[TIRE_FR].dir = FOR; 01412 motor[TIRE_FL].pwm = fast; 01413 motor[TIRE_FL].dir = BACK; 01414 motor[TIRE_BR].pwm = fast; 01415 motor[TIRE_BR].dir = FOR; 01416 motor[TIRE_BL].pwm = slow; 01417 motor[TIRE_BL].dir = BACK; 01418 mtc=true; 01419 break; 01420 case 3: 01421 motor[TIRE_FR].pwm = 0; 01422 motor[TIRE_FR].dir = FREE; 01423 motor[TIRE_FL].pwm = slow; 01424 motor[TIRE_FL].dir = BACK; 01425 motor[TIRE_BR].pwm = slow; 01426 motor[TIRE_BR].dir = FOR; 01427 motor[TIRE_BL].pwm = 0; 01428 motor[TIRE_BL].dir = FREE; 01429 mtc=true; 01430 break; 01431 case 2: 01432 motor[TIRE_FR].pwm = 0; 01433 motor[TIRE_FR].dir = FREE; 01434 motor[TIRE_FL].pwm = normal; 01435 motor[TIRE_FL].dir = BACK; 01436 motor[TIRE_BR].pwm = normal; 01437 motor[TIRE_BR].dir = FOR; 01438 motor[TIRE_BL].pwm = 0; 01439 motor[TIRE_BL].dir = FREE; 01440 mtc=true; 01441 break; 01442 } 01443 if(g[0]!=98&&mtc==true) { 01444 switch(g[0]) { 01445 case 0: 01446 mtc=false; 01447 break; 01448 case 255: 01449 motor[TIRE_FL].pwm += 5; 01450 mtc=false; 01451 break; 01452 case 253: 01453 motor[TIRE_FL].pwm += 10; 01454 mtc=false; 01455 break; 01456 case 254: 01457 motor[TIRE_FL].pwm += 20; 01458 motor[TIRE_FR].pwm = 0; 01459 mtc=false; 01460 break; 01461 case 1: 01462 motor[TIRE_FR].pwm += 5; 01463 mtc=false; 01464 break; 01465 case 3: 01466 motor[TIRE_FR].pwm += 10; 01467 mtc=false; 01468 break; 01469 case 2: 01470 motor[TIRE_FR].pwm += 20; 01471 motor[TIRE_FL].pwm = 0; 01472 mtc=false; 01473 break; 01474 } 01475 } 01476 } else if(mode==11) { 01477 motor[TIRE_FR].pwm = normal; 01478 motor[TIRE_FR].dir = FOR; 01479 motor[TIRE_FL].pwm = 0; 01480 motor[TIRE_FL].dir = FREE; 01481 motor[TIRE_BR].pwm = 0; 01482 motor[TIRE_BR].dir = FREE; 01483 motor[TIRE_BL].pwm = normal; 01484 motor[TIRE_BL].dir = BACK; 01485 if(g[2]==0) { 01486 count=0; 01487 cross=0; 01488 mode=20; 01489 } 01490 } else if(mode==20) { 01491 01492 if(Twsh==2) { 01493 solenoid.solenoid2 = SOLENOID_ON; 01494 } 01495 01496 if(counts==false&&g[1]==0) { 01497 cros++; 01498 counts=true; 01499 } 01500 if(counts==true&&g[1]==99) { 01501 counts=false; 01502 } 01503 01504 if(cros==2) { 01505 //mode=21; 01506 //count=0; 01507 //cros=0; 01508 }//ゆっくりモードに入れなかった時の保険 01509 if(g[1]==0) { 01510 fast=35; 01511 normal=20; 01512 slow=15; 01513 } 01514 switch(g[2]) { 01515 case 98: 01516 motor[TIRE_FR].pwm = normal; 01517 motor[TIRE_FR].dir = FOR; 01518 motor[TIRE_FL].pwm = normal; 01519 motor[TIRE_FL].dir = FOR; 01520 motor[TIRE_BR].pwm = normal; 01521 motor[TIRE_BR].dir = BACK; 01522 motor[TIRE_BL].pwm = normal; 01523 motor[TIRE_BL].dir = BACK; 01524 if(count>50000) { 01525 cross++; 01526 count=0; 01527 } 01528 mtc2=true; 01529 if(cross==2) { 01530 mode=22; 01531 cross=0; 01532 count=0; 01533 fast=40; 01534 normal=30; 01535 slow=20; 01536 } 01537 break; 01538 case 0: 01539 motor[TIRE_FR].pwm = fast; 01540 motor[TIRE_FR].dir = FOR; 01541 motor[TIRE_FL].pwm = fast; 01542 motor[TIRE_FL].dir = FOR; 01543 motor[TIRE_BR].pwm = fast; 01544 motor[TIRE_BR].dir = BACK; 01545 motor[TIRE_BL].pwm = fast; 01546 motor[TIRE_BL].dir = BACK; 01547 mtc2=true; 01548 break; 01549 case 255: 01550 motor[TIRE_FR].pwm = normal; 01551 motor[TIRE_FR].dir = FOR; 01552 motor[TIRE_FL].pwm = fast; 01553 motor[TIRE_FL].dir = FOR; 01554 motor[TIRE_BR].pwm = fast; 01555 motor[TIRE_BR].dir = BACK; 01556 motor[TIRE_BL].pwm = normal; 01557 motor[TIRE_BL].dir = BACK; 01558 mtc2=true; 01559 break; 01560 case 253: 01561 motor[TIRE_FR].pwm = 0; 01562 motor[TIRE_FR].dir = FREE; 01563 motor[TIRE_FL].pwm = slow; 01564 motor[TIRE_FL].dir = FOR; 01565 motor[TIRE_BR].pwm = slow; 01566 motor[TIRE_BR].dir = BACK; 01567 motor[TIRE_BL].pwm = 0; 01568 motor[TIRE_BL].dir = FREE; 01569 mtc2=true; 01570 break; 01571 case 254: 01572 motor[TIRE_FR].pwm = 0; 01573 motor[TIRE_FR].dir = FREE; 01574 motor[TIRE_FL].pwm = normal; 01575 motor[TIRE_FL].dir = FOR; 01576 motor[TIRE_BR].pwm = normal; 01577 motor[TIRE_BR].dir = BACK; 01578 motor[TIRE_BL].pwm = 0; 01579 motor[TIRE_BL].dir = FREE; 01580 mtc2=true; 01581 break; 01582 case 1: 01583 motor[TIRE_FR].pwm = fast; 01584 motor[TIRE_FR].dir = FOR; 01585 motor[TIRE_FL].pwm = slow; 01586 motor[TIRE_FL].dir = FOR; 01587 motor[TIRE_BR].pwm = slow; 01588 motor[TIRE_BR].dir = BACK; 01589 motor[TIRE_BL].pwm = fast; 01590 motor[TIRE_BL].dir = BACK; 01591 mtc2=true; 01592 break; 01593 case 3: 01594 motor[TIRE_FR].pwm = slow; 01595 motor[TIRE_FR].dir = FOR; 01596 motor[TIRE_FL].pwm = 0; 01597 motor[TIRE_FL].dir = FREE; 01598 motor[TIRE_BR].pwm = 0; 01599 motor[TIRE_BR].dir = FREE; 01600 motor[TIRE_BL].pwm = slow; 01601 motor[TIRE_BL].dir = BACK; 01602 mtc2=true; 01603 break; 01604 case 2: 01605 motor[TIRE_FR].pwm = normal; 01606 motor[TIRE_FR].dir = FOR; 01607 motor[TIRE_FL].pwm = 0; 01608 motor[TIRE_FL].dir = FREE; 01609 motor[TIRE_BR].pwm = 0; 01610 motor[TIRE_BR].dir = FREE; 01611 motor[TIRE_BL].pwm = normal; 01612 motor[TIRE_BL].dir = BACK; 01613 mtc2=true; 01614 break; 01615 } 01616 if(g[2]!=98&&mtc2==true) { 01617 switch(g[3]) { 01618 case 0: 01619 mtc2=false; 01620 break; 01621 case 255: 01622 motor[TIRE_BL].pwm += 5; 01623 mtc2=false; 01624 break; 01625 case 253: 01626 motor[TIRE_BL].pwm += 10; 01627 mtc2=false; 01628 break; 01629 case 254: 01630 motor[TIRE_BL].pwm += 20; 01631 motor[TIRE_FL].pwm = 0; 01632 mtc2=false; 01633 break; 01634 case 1: 01635 motor[TIRE_FL].pwm += 5; 01636 mtc2=false; 01637 break; 01638 case 3: 01639 motor[TIRE_FL].pwm += 10; 01640 mtc2=false; 01641 break; 01642 case 2: 01643 motor[TIRE_FL].pwm += 20; 01644 motor[TIRE_BL].pwm = 0; 01645 mtc2=false; 01646 break; 01647 } 01648 } 01649 01650 } else if(mode==21) { 01651 motor[TIRE_FR].pwm = 30; 01652 motor[TIRE_FR].dir = BRAKE; 01653 motor[TIRE_FL].pwm = 30; 01654 motor[TIRE_FL].dir = BRAKE; 01655 motor[TIRE_BR].pwm = 30; 01656 motor[TIRE_BR].dir = BRAKE; 01657 motor[TIRE_BL].pwm = 30; 01658 motor[TIRE_BL].dir = BRAKE; 01659 if(count<=100000) { 01660 mode=22; 01661 } 01662 } 01663 kiri=true; 01664 if(kiri) { 01665 if(mode==22) { 01666 motor[TIRE_FR].pwm = 100; 01667 motor[TIRE_FR].dir = BRAKE; 01668 motor[TIRE_FL].pwm = 100; 01669 motor[TIRE_FL].dir = BRAKE; 01670 motor[TIRE_BR].pwm = 100; 01671 motor[TIRE_BR].dir = BRAKE; 01672 motor[TIRE_BL].pwm = 100; 01673 motor[TIRE_BL].dir = BRAKE; 01674 if(count>=10000) { 01675 mode=23; 01676 } 01677 } else if(mode==23) { 01678 motor[TIRE_FR].pwm = 20; 01679 motor[TIRE_FR].dir = BACK; 01680 motor[TIRE_FL].pwm = 20; 01681 motor[TIRE_FL].dir = BACK; 01682 motor[TIRE_BR].pwm = 20; 01683 motor[TIRE_BR].dir = FOR; 01684 motor[TIRE_BL].pwm = 20; 01685 motor[TIRE_BL].dir = FOR; 01686 if(g[2]==98) { 01687 mode=24; 01688 count=0; 01689 } 01690 } else if(mode==24) { 01691 motor[TIRE_FR].pwm = 20; 01692 motor[TIRE_FR].dir = BACK; 01693 motor[TIRE_FL].pwm = 20; 01694 motor[TIRE_FL].dir = BACK; 01695 motor[TIRE_BR].pwm = 20; 01696 motor[TIRE_BR].dir = FOR; 01697 motor[TIRE_BL].pwm = 20; 01698 motor[TIRE_BL].dir = FOR; 01699 if(g[2]!=98) { 01700 count=0; 01701 mode=25; 01702 } 01703 } else if(mode==25) { 01704 motor[TIRE_FR].pwm = 10; 01705 motor[TIRE_FR].dir = BRAKE; 01706 motor[TIRE_FL].pwm = 10; 01707 motor[TIRE_FL].dir = BRAKE; 01708 motor[TIRE_BR].pwm = 10; 01709 motor[TIRE_BR].dir = BRAKE; 01710 motor[TIRE_BL].pwm = 10; 01711 motor[TIRE_BL].dir = BRAKE; 01712 if(count>=10000) { 01713 mode=26; 01714 count=0; 01715 } 01716 } else if(mode==26) { 01717 motor[TIRE_FR].pwm = 20; 01718 motor[TIRE_FR].dir = FOR; 01719 motor[TIRE_FL].pwm = 20; 01720 motor[TIRE_FL].dir = FOR; 01721 motor[TIRE_BR].pwm = 20; 01722 motor[TIRE_BR].dir = BACK; 01723 motor[TIRE_BL].pwm = 20; 01724 motor[TIRE_BL].dir = BACK; 01725 if(count>=5000) { 01726 mode=27; 01727 count=0; 01728 } 01729 } else if(mode==27) { 01730 motor[TIRE_FR].pwm = 20; 01731 motor[TIRE_FR].dir = FOR; 01732 motor[TIRE_FL].pwm = 20; 01733 motor[TIRE_FL].dir = FOR; 01734 motor[TIRE_BR].pwm = 20; 01735 motor[TIRE_BR].dir = FOR; 01736 motor[TIRE_BL].pwm = 20; 01737 motor[TIRE_BL].dir = FOR; 01738 //どれくらい回転するかって値 01739 if(count>=4000) { 01740 count=0; 01741 mode=28; 01742 } 01743 } else if(mode==28) { 01744 motor[TIRE_FR].pwm = 10; 01745 motor[TIRE_FR].dir = BRAKE; 01746 motor[TIRE_FL].pwm = 10; 01747 motor[TIRE_FL].dir = BRAKE; 01748 motor[TIRE_BR].pwm = 10; 01749 motor[TIRE_BR].dir = BRAKE; 01750 motor[TIRE_BL].pwm = 10; 01751 motor[TIRE_BL].dir = BRAKE; 01752 if(count>=10000) { 01753 mode=30; 01754 count=0; 01755 } 01756 } 01757 } else { 01758 if(mode==22) { 01759 motor[TIRE_FR].pwm = 0; 01760 motor[TIRE_FR].dir = FREE; 01761 motor[TIRE_FL].pwm = 25; 01762 motor[TIRE_FL].dir = FOR; 01763 motor[TIRE_BR].pwm = 25; 01764 motor[TIRE_BR].dir = BACK; 01765 motor[TIRE_BL].pwm = 0; 01766 motor[TIRE_BL].dir = FREE; 01767 if(g[0]==0&&count>=10000) { 01768 mode=30; 01769 count=0; 01770 cros=0; 01771 } 01772 } 01773 } 01774 if(mode==30) { 01775 motor[TIRE_FR].pwm = 60; 01776 motor[TIRE_FR].dir = FOR; 01777 motor[TIRE_FL].pwm = 60; 01778 motor[TIRE_FL].dir = BACK; 01779 motor[TIRE_BR].pwm = 60; 01780 motor[TIRE_BR].dir = FOR; 01781 motor[TIRE_BL].pwm = 57; 01782 motor[TIRE_BL].dir = BACK; 01783 if(g[1]==98) { 01784 mode=31; 01785 count=0; 01786 } 01787 } else if(mode==31) { 01788 motor[TIRE_FR].pwm = 0; 01789 motor[TIRE_FR].dir = FREE; 01790 motor[TIRE_FL].pwm = 0; 01791 motor[TIRE_FL].dir = FREE; 01792 motor[TIRE_BR].pwm = 0; 01793 motor[TIRE_BR].dir = FREE; 01794 motor[TIRE_BL].pwm = 0; 01795 motor[TIRE_BL].dir = FREE; 01796 if(count>=5000) { 01797 mode=32; 01798 count=0; 01799 } 01800 } else if(mode==32) { 01801 motor[TIRE_FR].pwm = 10; 01802 motor[TIRE_FR].dir = BRAKE; 01803 motor[TIRE_FL].pwm = 10; 01804 motor[TIRE_FL].dir = BRAKE; 01805 motor[TIRE_BR].pwm = 10; 01806 motor[TIRE_BR].dir = BRAKE; 01807 motor[TIRE_BL].pwm = 10; 01808 motor[TIRE_BL].dir = BRAKE; 01809 if(count>=30000) { 01810 mode=33; 01811 count=0; 01812 } 01813 } else if(mode==33) { 01814 motor[TIRE_FR].pwm = 30; 01815 motor[TIRE_FR].dir = FOR; 01816 motor[TIRE_FL].pwm = 0; 01817 motor[TIRE_FL].dir = FREE; 01818 motor[TIRE_BR].pwm = 0; 01819 motor[TIRE_BR].dir = FREE; 01820 motor[TIRE_BL].pwm = 30; 01821 motor[TIRE_BL].dir = BACK; 01822 if(g[3]==0&&count>=20000) { 01823 count=0; 01824 if(Twsh==2) { 01825 solenoid.solenoid2 = SOLENOID_OFF; 01826 } 01827 mode=40; 01828 } 01829 } else if(mode==40) { 01830 if(LIB) { 01831 mode=42; 01832 } 01833 switch(g[3]) { 01834 case 98: 01835 motor[TIRE_FR].pwm = normal; 01836 motor[TIRE_FR].dir = BACK; 01837 motor[TIRE_FL].pwm = normal; 01838 motor[TIRE_FL].dir = BACK; 01839 motor[TIRE_BR].pwm = normal; 01840 motor[TIRE_BR].dir = FOR; 01841 motor[TIRE_BL].pwm = normal; 01842 motor[TIRE_BL].dir = FOR; 01843 if(count>=10000) { 01844 cross++; 01845 count=0; 01846 } 01847 mtc2=true; 01848 if(cross==2) { 01849 mode=41; 01850 count=0; 01851 cross=0; 01852 } 01853 break; 01854 case 0: 01855 motor[TIRE_FR].pwm = fast; 01856 motor[TIRE_FR].dir = BACK; 01857 motor[TIRE_FL].pwm = fast; 01858 motor[TIRE_FL].dir = BACK; 01859 motor[TIRE_BR].pwm = fast; 01860 motor[TIRE_BR].dir = FOR; 01861 motor[TIRE_BL].pwm = fast; 01862 motor[TIRE_BL].dir = FOR; 01863 mtc2=true; 01864 break; 01865 case 255: 01866 motor[TIRE_FR].pwm = slow; 01867 motor[TIRE_FR].dir = BACK; 01868 motor[TIRE_FL].pwm = fast; 01869 motor[TIRE_FL].dir = BACK; 01870 motor[TIRE_BR].pwm = fast; 01871 motor[TIRE_BR].dir = FOR; 01872 motor[TIRE_BL].pwm = slow; 01873 motor[TIRE_BL].dir = FOR; 01874 mtc2=true; 01875 break; 01876 case 253: 01877 motor[TIRE_FR].pwm = 0; 01878 motor[TIRE_FR].dir = FREE; 01879 motor[TIRE_FL].pwm = slow; 01880 motor[TIRE_FL].dir = BACK; 01881 motor[TIRE_BR].pwm = slow; 01882 motor[TIRE_BR].dir = FOR; 01883 motor[TIRE_BL].pwm = 0; 01884 motor[TIRE_BL].dir = FREE; 01885 mtc2=true; 01886 break; 01887 case 254: 01888 motor[TIRE_FR].pwm = 0; 01889 motor[TIRE_FR].dir = FREE; 01890 motor[TIRE_FL].pwm = normal; 01891 motor[TIRE_FL].dir = BACK; 01892 motor[TIRE_BR].pwm = normal; 01893 motor[TIRE_BR].dir = FOR; 01894 motor[TIRE_BL].pwm = 0; 01895 motor[TIRE_BL].dir = FREE; 01896 mtc2=true; 01897 break; 01898 case 1: 01899 motor[TIRE_FR].pwm = fast; 01900 motor[TIRE_FR].dir = BACK; 01901 motor[TIRE_FL].pwm = slow; 01902 motor[TIRE_FL].dir = BACK; 01903 motor[TIRE_BR].pwm = slow; 01904 motor[TIRE_BR].dir = FOR; 01905 motor[TIRE_BL].pwm = fast; 01906 motor[TIRE_BL].dir = FOR; 01907 mtc2=true; 01908 break; 01909 case 3: 01910 motor[TIRE_FR].pwm = slow; 01911 motor[TIRE_FR].dir = BACK; 01912 motor[TIRE_FL].pwm = 0; 01913 motor[TIRE_FL].dir = FREE; 01914 motor[TIRE_BR].pwm = 0; 01915 motor[TIRE_BR].dir = FREE; 01916 motor[TIRE_BL].pwm = slow; 01917 motor[TIRE_BL].dir = FOR; 01918 mtc2=true; 01919 break; 01920 case 2: 01921 motor[TIRE_FR].pwm = normal; 01922 motor[TIRE_FR].dir = BACK; 01923 motor[TIRE_FL].pwm = 0; 01924 motor[TIRE_FL].dir = FREE; 01925 motor[TIRE_BR].pwm = 0; 01926 motor[TIRE_BR].dir = FREE; 01927 motor[TIRE_BL].pwm = normal; 01928 motor[TIRE_BL].dir = FOR; 01929 mtc2=true; 01930 break; 01931 } 01932 if(g[2]!=98&&mtc2==true) { 01933 switch(g[2]) { 01934 case 0: 01935 mtc2=false; 01936 break; 01937 case 255: 01938 motor[TIRE_FR].pwm += 5; 01939 mtc2=false; 01940 break; 01941 case 253: 01942 motor[TIRE_FR].pwm += 10; 01943 mtc2=false; 01944 break; 01945 case 254: 01946 motor[TIRE_FR].pwm += 20; 01947 motor[TIRE_BR].pwm = 0; 01948 mtc2=false; 01949 break; 01950 case 1: 01951 motor[TIRE_BR].pwm += 5; 01952 mtc2=false; 01953 break; 01954 case 3: 01955 motor[TIRE_BR].pwm += 10; 01956 mtc2=false; 01957 break; 01958 case 2: 01959 motor[TIRE_BR].pwm += 20; 01960 motor[TIRE_FR].pwm = 0; 01961 mtc2=false; 01962 break; 01963 } 01964 } 01965 } else if(mode==41) { 01966 if(LIF||LIB) { 01967 mode=42; 01968 } 01969 motor[TIRE_FR].pwm = 30; 01970 motor[TIRE_FR].dir = BACK; 01971 motor[TIRE_FL].pwm = 0; 01972 motor[TIRE_FL].dir = FREE; 01973 motor[TIRE_BR].pwm = 0; 01974 motor[TIRE_BR].dir = FREE; 01975 motor[TIRE_BL].pwm = 30; 01976 motor[TIRE_BL].dir = FOR; 01977 if(g[1]==0) { 01978 count=0; 01979 mode=70; 01980 normal=60; 01981 fast=70; 01982 slow=40; 01983 } 01984 } else if(mode==42) { 01985 motor[TIRE_FR].pwm = 0; 01986 motor[TIRE_FR].dir = FREE; 01987 motor[TIRE_FL].pwm = 20; 01988 motor[TIRE_FL].dir = FOR; 01989 motor[TIRE_BR].pwm = 20; 01990 motor[TIRE_BR].dir = BACK; 01991 motor[TIRE_BL].pwm = 0; 01992 motor[TIRE_BL].dir = FREE; 01993 if(g[0]==0) { 01994 count=0; 01995 slow=40; 01996 } 01997 01998 } else if(mode==70) 01999 switch(g[0]) { 02000 case 99: 02001 if(count>=100000) { 02002 cross++; 02003 mode=100; 02004 count=0; 02005 } 02006 if(cross==2) { 02007 cross=0; 02008 mode=100; 02009 } 02010 break; 02011 case 98: 02012 motor[TIRE_FR].pwm = normal; 02013 motor[TIRE_FR].dir = BACK; 02014 motor[TIRE_FL].pwm = normal; 02015 motor[TIRE_FL].dir = FOR; 02016 motor[TIRE_BR].pwm = normal; 02017 motor[TIRE_BR].dir = BACK; 02018 motor[TIRE_BL].pwm = normal; 02019 motor[TIRE_BL].dir = FOR; 02020 break; 02021 case 0: 02022 motor[TIRE_FR].pwm = fast; 02023 motor[TIRE_FR].dir = BACK; 02024 motor[TIRE_FL].pwm = fast; 02025 motor[TIRE_FL].dir = FOR; 02026 motor[TIRE_BR].pwm = fast; 02027 motor[TIRE_BR].dir = BACK; 02028 motor[TIRE_BL].pwm = fast; 02029 motor[TIRE_BL].dir = FOR; 02030 mtc=true; 02031 break; 02032 case 255: 02033 motor[TIRE_FR].pwm = fast; 02034 motor[TIRE_FR].dir = BACK; 02035 motor[TIRE_FL].pwm = slow; 02036 motor[TIRE_FL].dir = FOR; 02037 motor[TIRE_BR].pwm = slow; 02038 motor[TIRE_BR].dir = BACK; 02039 motor[TIRE_BL].pwm = fast; 02040 motor[TIRE_BL].dir = FOR; 02041 mtc=true; 02042 break; 02043 case 253: 02044 motor[TIRE_FR].pwm = slow; 02045 motor[TIRE_FR].dir = BACK; 02046 motor[TIRE_FL].pwm = 0; 02047 motor[TIRE_FL].dir = FREE; 02048 motor[TIRE_BR].pwm = 0; 02049 motor[TIRE_BR].dir = FREE; 02050 motor[TIRE_BL].pwm = slow; 02051 motor[TIRE_BL].dir = FOR; 02052 mtc=true; 02053 break; 02054 case 254: 02055 motor[TIRE_FR].pwm = normal; 02056 motor[TIRE_FR].dir = BACK; 02057 motor[TIRE_FL].pwm = 0; 02058 motor[TIRE_FL].dir = FREE; 02059 motor[TIRE_BR].pwm = 0; 02060 motor[TIRE_BR].dir = FREE; 02061 motor[TIRE_BL].pwm = normal; 02062 motor[TIRE_BL].dir = FOR; 02063 mtc=true; 02064 break; 02065 case 1: 02066 motor[TIRE_FR].pwm = slow; 02067 motor[TIRE_FR].dir = BACK; 02068 motor[TIRE_FL].pwm = fast; 02069 motor[TIRE_FL].dir = FOR; 02070 motor[TIRE_BR].pwm = fast; 02071 motor[TIRE_BR].dir = BACK; 02072 motor[TIRE_BL].pwm = slow; 02073 motor[TIRE_BL].dir = FOR; 02074 mtc=true; 02075 break; 02076 case 3: 02077 motor[TIRE_FR].pwm = 0; 02078 motor[TIRE_FR].dir = FREE; 02079 motor[TIRE_FL].pwm = slow; 02080 motor[TIRE_FL].dir = FOR; 02081 motor[TIRE_BR].pwm = slow; 02082 motor[TIRE_BR].dir = BACK; 02083 motor[TIRE_BL].pwm = 0; 02084 motor[TIRE_BL].dir = FREE; 02085 mtc=true; 02086 break; 02087 case 2: 02088 motor[TIRE_FR].pwm = 0; 02089 motor[TIRE_FR].dir = FREE; 02090 motor[TIRE_FL].pwm = normal; 02091 motor[TIRE_FL].dir = FOR; 02092 motor[TIRE_BR].pwm = normal; 02093 motor[TIRE_BR].dir = BACK; 02094 motor[TIRE_BL].pwm = 0; 02095 motor[TIRE_BL].dir = FREE; 02096 mtc=true; 02097 break; 02098 } 02099 if(g[1]!=98&&mtc==true) { 02100 switch(g[1]) { 02101 case 0: 02102 mtc=false; 02103 break; 02104 case 255: 02105 motor[TIRE_BR].pwm += 5; 02106 mtc=false; 02107 break; 02108 case 253: 02109 motor[TIRE_BR].pwm += 10; 02110 mtc=false; 02111 break; 02112 case 254: 02113 motor[TIRE_BR].pwm += 20; 02114 motor[TIRE_BL].pwm = 0; 02115 mtc=false; 02116 break; 02117 case 1: 02118 motor[TIRE_BL].pwm += 5; 02119 mtc=false; 02120 break; 02121 case 3: 02122 motor[TIRE_BL].pwm += 10; 02123 mtc=false; 02124 break; 02125 case 2: 02126 motor[TIRE_BL].pwm += 20; 02127 motor[TIRE_BR].pwm = 0; 02128 mtc=false; 02129 break; 02130 } 02131 } else if(mode==100) { 02132 motor[TIRE_FR].pwm = 20; 02133 motor[TIRE_FR].dir = BACK; 02134 motor[TIRE_FL].pwm = 20; 02135 motor[TIRE_FL].dir = FOR; 02136 motor[TIRE_BR].pwm = 23; 02137 motor[TIRE_BR].dir = BACK; 02138 motor[TIRE_BL].pwm = 20; 02139 motor[TIRE_BL].dir = FOR; 02140 } 02141 } 02142 #endif 02143 02144 #if USE_PROCESS_NUM>5 02145 static void Process5() 02146 { 02147 } 02148 #endif 02149 02150 #if USE_PROCESS_NUM>6 02151 static void Process6() 02152 { 02153 // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]); 02154 LedMode(3); 02155 count++; 02156 if(mode==1) {//スタートゾーンから白線検知 02157 fast=40; 02158 normal=30; 02159 slow=20; 02160 motor[TIRE_FR].pwm = normal; 02161 motor[TIRE_FR].dir = BACK; 02162 motor[TIRE_FL].pwm = normal; 02163 motor[TIRE_FL].dir = FOR; 02164 motor[TIRE_BR].pwm = normal; 02165 motor[TIRE_BR].dir = BACK; 02166 motor[TIRE_BL].pwm = normal; 02167 motor[TIRE_BL].dir = FOR; 02168 if(g[2]==0&&count>=100000) { 02169 count=100000; 02170 cross=0; 02171 mode=3; 02172 } 02173 } else if(mode==3) { //横ライントレースから縦ライントレースへ 02174 motor[TIRE_FR].pwm = 0; 02175 motor[TIRE_FR].dir = FREE; 02176 motor[TIRE_FL].pwm = 25; 02177 motor[TIRE_FL].dir = FOR; 02178 motor[TIRE_BR].pwm = 25; 02179 motor[TIRE_BR].dir = BACK; 02180 motor[TIRE_BL].pwm = 0; 02181 motor[TIRE_BL].dir = FREE; 02182 if(g[0]==0) { 02183 mode=10; 02184 count=100000; 02185 } 02186 } else if(mode==10) { //縦ライントレース 02187 if(counts==false&&g[2]==0) { 02188 cros++; 02189 counts=true; 02190 } 02191 if(counts==true&&g[2]==99) { 02192 counts=false; 02193 } 02194 02195 if(cros==Twsh) { 02196 mode=11; 02197 count=0; 02198 cros=0; 02199 }//ゆっくりモードに入れなかった時の保険 02200 switch(g[0]) { 02201 case 98: 02202 motor[TIRE_FR].pwm = normal; 02203 motor[TIRE_FR].dir = BACK; 02204 motor[TIRE_FL].pwm = normal; 02205 motor[TIRE_FL].dir = FOR; 02206 motor[TIRE_BR].pwm = normal; 02207 motor[TIRE_BR].dir = BACK; 02208 motor[TIRE_BL].pwm = normal; 02209 motor[TIRE_BL].dir = FOR; 02210 mtc=true; 02211 if(count>80000) { 02212 cross++; 02213 count=0; 02214 } 02215 if(cross==Twsh) { 02216 mode=11; 02217 count=0; 02218 cross=0; 02219 } 02220 break; 02221 case 0: 02222 motor[TIRE_FR].pwm = fast; 02223 motor[TIRE_FR].dir = BACK; 02224 motor[TIRE_FL].pwm = fast; 02225 motor[TIRE_FL].dir = FOR; 02226 motor[TIRE_BR].pwm = fast; 02227 motor[TIRE_BR].dir = BACK; 02228 motor[TIRE_BL].pwm = fast; 02229 motor[TIRE_BL].dir = FOR; 02230 mtc=true; 02231 break; 02232 case 255: 02233 motor[TIRE_FR].pwm = fast; 02234 motor[TIRE_FR].dir = BACK; 02235 motor[TIRE_FL].pwm = slow; 02236 motor[TIRE_FL].dir = FOR; 02237 motor[TIRE_BR].pwm = slow; 02238 motor[TIRE_BR].dir = BACK; 02239 motor[TIRE_BL].pwm = fast; 02240 motor[TIRE_BL].dir = FOR; 02241 mtc=true; 02242 break; 02243 case 253: 02244 motor[TIRE_FR].pwm = slow; 02245 motor[TIRE_FR].dir = BACK; 02246 motor[TIRE_FL].pwm = 0; 02247 motor[TIRE_FL].dir = FREE; 02248 motor[TIRE_BR].pwm = 0; 02249 motor[TIRE_BR].dir = FREE; 02250 motor[TIRE_BL].pwm = slow; 02251 motor[TIRE_BL].dir = FOR; 02252 mtc=true; 02253 break; 02254 case 254: 02255 motor[TIRE_FR].pwm = normal; 02256 motor[TIRE_FR].dir = BACK; 02257 motor[TIRE_FL].pwm = 0; 02258 motor[TIRE_FL].dir = FREE; 02259 motor[TIRE_BR].pwm = 0; 02260 motor[TIRE_BR].dir = FREE; 02261 motor[TIRE_BL].pwm = normal; 02262 motor[TIRE_BL].dir = FOR; 02263 mtc=true; 02264 break; 02265 case 1: 02266 motor[TIRE_FR].pwm = slow; 02267 motor[TIRE_FR].dir = BACK; 02268 motor[TIRE_FL].pwm = fast; 02269 motor[TIRE_FL].dir = FOR; 02270 motor[TIRE_BR].pwm = fast; 02271 motor[TIRE_BR].dir = BACK; 02272 motor[TIRE_BL].pwm = slow; 02273 motor[TIRE_BL].dir = FOR; 02274 mtc=true; 02275 break; 02276 case 3: 02277 motor[TIRE_FR].pwm = 0; 02278 motor[TIRE_FR].dir = FREE; 02279 motor[TIRE_FL].pwm = slow; 02280 motor[TIRE_FL].dir = FOR; 02281 motor[TIRE_BR].pwm = slow; 02282 motor[TIRE_BR].dir = BACK; 02283 motor[TIRE_BL].pwm = 0; 02284 motor[TIRE_BL].dir = FREE; 02285 mtc=true; 02286 break; 02287 case 2: 02288 motor[TIRE_FR].pwm = 0; 02289 motor[TIRE_FR].dir = FREE; 02290 motor[TIRE_FL].pwm = normal; 02291 motor[TIRE_FL].dir = FOR; 02292 motor[TIRE_BR].pwm = normal; 02293 motor[TIRE_BR].dir = BACK; 02294 motor[TIRE_BL].pwm = 0; 02295 motor[TIRE_BL].dir = FREE; 02296 mtc=true; 02297 break; 02298 } 02299 if(g[0]!=98&&mtc==true) { 02300 switch(g[1]) { 02301 case 0: 02302 mtc=false; 02303 break; 02304 case 255: 02305 motor[TIRE_BR].pwm += 5; 02306 mtc=false; 02307 break; 02308 case 253: 02309 motor[TIRE_BR].pwm += 10; 02310 mtc=false; 02311 break; 02312 case 254: 02313 motor[TIRE_BR].pwm += 20; 02314 motor[TIRE_BL].pwm = 0; 02315 mtc=false; 02316 break; 02317 case 1: 02318 motor[TIRE_BL].pwm += 5; 02319 mtc=false; 02320 break; 02321 case 3: 02322 motor[TIRE_BL].pwm += 10; 02323 mtc=false; 02324 break; 02325 case 2: 02326 motor[TIRE_BL].pwm += 20; 02327 motor[TIRE_BR].pwm = 0; 02328 mtc=false; 02329 break; 02330 } 02331 } 02332 } else if(mode==11) { 02333 motor[TIRE_FR].pwm = 0; 02334 motor[TIRE_FR].dir = FREE; 02335 motor[TIRE_FL].pwm = normal; 02336 motor[TIRE_FL].dir = FOR; 02337 motor[TIRE_BR].pwm = normal; 02338 motor[TIRE_BR].dir = BACK; 02339 motor[TIRE_BL].pwm = 0; 02340 motor[TIRE_BL].dir = FREE; 02341 if(g[2]==0) { 02342 count=0; 02343 cross=0; 02344 mode=20; 02345 } 02346 } else if(mode==20) { 02347 02348 if(Twsh==2) { 02349 solenoid.solenoid2 = SOLENOID_ON; 02350 } 02351 if(counts==false&&g[0]==0) { 02352 cros++; 02353 counts=true; 02354 } 02355 if(counts==true&&g[0]==99) { 02356 counts=false; 02357 } 02358 02359 if(cros==2) { 02360 02361 // mode=21; 02362 // count=0; 02363 // cros=0; 02364 }//ゆっくりモードに入れなかった時の保険 02365 if(g[0]==0) { 02366 fast=35; 02367 normal=20; 02368 slow=15; 02369 } 02370 switch(g[2]) { 02371 case 98: 02372 motor[TIRE_FR].pwm = normal; 02373 motor[TIRE_FR].dir = FOR; 02374 motor[TIRE_FL].pwm = normal; 02375 motor[TIRE_FL].dir = FOR; 02376 motor[TIRE_BR].pwm = normal; 02377 motor[TIRE_BR].dir = BACK; 02378 motor[TIRE_BL].pwm = normal; 02379 motor[TIRE_BL].dir = BACK; 02380 if(count>30000) { 02381 cross++; 02382 count=0; 02383 } 02384 mtc2=true; 02385 if(cross==2) { 02386 mode=22; 02387 count=0; 02388 cross=0; 02389 fast=40; 02390 normal=30; 02391 slow=20; 02392 } 02393 break; 02394 case 0: 02395 motor[TIRE_FR].pwm = fast; 02396 motor[TIRE_FR].dir = FOR; 02397 motor[TIRE_FL].pwm = fast; 02398 motor[TIRE_FL].dir = FOR; 02399 motor[TIRE_BR].pwm = fast; 02400 motor[TIRE_BR].dir = BACK; 02401 motor[TIRE_BL].pwm = fast; 02402 motor[TIRE_BL].dir = BACK; 02403 mtc2=true; 02404 break; 02405 case 255: 02406 motor[TIRE_FR].pwm = normal; 02407 motor[TIRE_FR].dir = FOR; 02408 motor[TIRE_FL].pwm = fast; 02409 motor[TIRE_FL].dir = FOR; 02410 motor[TIRE_BR].pwm = fast; 02411 motor[TIRE_BR].dir = BACK; 02412 motor[TIRE_BL].pwm = normal; 02413 motor[TIRE_BL].dir = BACK; 02414 mtc2=true; 02415 break; 02416 case 253: 02417 motor[TIRE_FR].pwm = 0; 02418 motor[TIRE_FR].dir = FREE; 02419 motor[TIRE_FL].pwm = slow; 02420 motor[TIRE_FL].dir = FOR; 02421 motor[TIRE_BR].pwm = slow; 02422 motor[TIRE_BR].dir = BACK; 02423 motor[TIRE_BL].pwm = 0; 02424 motor[TIRE_BL].dir = FREE; 02425 mtc2=true; 02426 break; 02427 case 254: 02428 motor[TIRE_FR].pwm = 0; 02429 motor[TIRE_FR].dir = FREE; 02430 motor[TIRE_FL].pwm = normal; 02431 motor[TIRE_FL].dir = FOR; 02432 motor[TIRE_BR].pwm = normal; 02433 motor[TIRE_BR].dir = BACK; 02434 motor[TIRE_BL].pwm = 0; 02435 motor[TIRE_BL].dir = FREE; 02436 mtc2=true; 02437 break; 02438 case 1: 02439 motor[TIRE_FR].pwm = fast; 02440 motor[TIRE_FR].dir = FOR; 02441 motor[TIRE_FL].pwm = slow; 02442 motor[TIRE_FL].dir = FOR; 02443 motor[TIRE_BR].pwm = slow; 02444 motor[TIRE_BR].dir = BACK; 02445 motor[TIRE_BL].pwm = fast; 02446 motor[TIRE_BL].dir = BACK; 02447 mtc2=true; 02448 break; 02449 case 3: 02450 motor[TIRE_FR].pwm = slow; 02451 motor[TIRE_FR].dir = FOR; 02452 motor[TIRE_FL].pwm = 0; 02453 motor[TIRE_FL].dir = FREE; 02454 motor[TIRE_BR].pwm = 0; 02455 motor[TIRE_BR].dir = FREE; 02456 motor[TIRE_BL].pwm = slow; 02457 motor[TIRE_BL].dir = BACK; 02458 mtc2=true; 02459 break; 02460 case 2: 02461 motor[TIRE_FR].pwm = normal; 02462 motor[TIRE_FR].dir = FOR; 02463 motor[TIRE_FL].pwm = 0; 02464 motor[TIRE_FL].dir = FREE; 02465 motor[TIRE_BR].pwm = 0; 02466 motor[TIRE_BR].dir = FREE; 02467 motor[TIRE_BL].pwm = normal; 02468 motor[TIRE_BL].dir = BACK; 02469 mtc2=true; 02470 break; 02471 } 02472 if(g[2]!=98&&mtc2==true) { 02473 switch(g[3]) { 02474 case 0: 02475 mtc2=false; 02476 break; 02477 case 255: 02478 motor[TIRE_BL].pwm += 5; 02479 mtc2=false; 02480 break; 02481 case 253: 02482 motor[TIRE_BL].pwm += 10; 02483 mtc2=false; 02484 break; 02485 case 254: 02486 motor[TIRE_BL].pwm += 20; 02487 motor[TIRE_FL].pwm = 0; 02488 mtc2=false; 02489 break; 02490 case 1: 02491 motor[TIRE_FL].pwm += 5; 02492 mtc2=false; 02493 break; 02494 case 3: 02495 motor[TIRE_FL].pwm += 10; 02496 mtc2=false; 02497 break; 02498 case 2: 02499 motor[TIRE_FL].pwm += 20; 02500 motor[TIRE_BL].pwm = 0; 02501 mtc2=false; 02502 break; 02503 } 02504 } 02505 } else if(mode==21) { 02506 motor[TIRE_FR].pwm = 30; 02507 motor[TIRE_FR].dir = FOR; 02508 motor[TIRE_FL].pwm = 30; 02509 motor[TIRE_FL].dir = FOR; 02510 motor[TIRE_BR].pwm = 30; 02511 motor[TIRE_BR].dir = BACK; 02512 motor[TIRE_BL].pwm = 30; 02513 motor[TIRE_BL].dir = BACK; 02514 if(g[2]==98&&count>=10000) { 02515 mode=22; 02516 count=0; 02517 } 02518 } 02519 kiri=true; 02520 if(kiri) { 02521 if(mode==22) { 02522 motor[TIRE_FR].pwm = 100; 02523 motor[TIRE_FR].dir = BRAKE; 02524 motor[TIRE_FL].pwm = 100; 02525 motor[TIRE_FL].dir = BRAKE; 02526 motor[TIRE_BR].pwm = 100; 02527 motor[TIRE_BR].dir = BRAKE; 02528 motor[TIRE_BL].pwm = 100; 02529 motor[TIRE_BL].dir = BRAKE; 02530 if(count>=10000) { 02531 mode=23; 02532 } 02533 } else if(mode==23) { 02534 motor[TIRE_FR].pwm = 20; 02535 motor[TIRE_FR].dir = BACK; 02536 motor[TIRE_FL].pwm = 20; 02537 motor[TIRE_FL].dir = BACK; 02538 motor[TIRE_BR].pwm = 20; 02539 motor[TIRE_BR].dir = FOR; 02540 motor[TIRE_BL].pwm = 20; 02541 motor[TIRE_BL].dir = FOR; 02542 if(g[2]==98) { 02543 mode=24; 02544 count=0; 02545 } 02546 } else if(mode==24) { 02547 motor[TIRE_FR].pwm = 20; 02548 motor[TIRE_FR].dir = BACK; 02549 motor[TIRE_FL].pwm = 20; 02550 motor[TIRE_FL].dir = BACK; 02551 motor[TIRE_BR].pwm = 20; 02552 motor[TIRE_BR].dir = FOR; 02553 motor[TIRE_BL].pwm = 20; 02554 motor[TIRE_BL].dir = FOR; 02555 if(g[2]!=98) { 02556 count=0; 02557 mode=25; 02558 } 02559 } else if(mode==25) { 02560 motor[TIRE_FR].pwm = 10; 02561 motor[TIRE_FR].dir = BRAKE; 02562 motor[TIRE_FL].pwm = 10; 02563 motor[TIRE_FL].dir = BRAKE; 02564 motor[TIRE_BR].pwm = 10; 02565 motor[TIRE_BR].dir = BRAKE; 02566 motor[TIRE_BL].pwm = 10; 02567 motor[TIRE_BL].dir = BRAKE; 02568 if(count>=10000) { 02569 mode=26; 02570 count=0; 02571 } 02572 } else if(mode==26) { 02573 motor[TIRE_FR].pwm = 20; 02574 motor[TIRE_FR].dir = FOR; 02575 motor[TIRE_FL].pwm = 20; 02576 motor[TIRE_FL].dir = FOR; 02577 motor[TIRE_BR].pwm = 20; 02578 motor[TIRE_BR].dir = BACK; 02579 motor[TIRE_BL].pwm = 20; 02580 motor[TIRE_BL].dir = BACK; 02581 if(count>=5000) { 02582 mode=27; 02583 count=0; 02584 } 02585 } else if(mode==27) { 02586 motor[TIRE_FR].pwm = 20; 02587 motor[TIRE_FR].dir = FOR; 02588 motor[TIRE_FL].pwm = 20; 02589 motor[TIRE_FL].dir = FOR; 02590 motor[TIRE_BR].pwm = 20; 02591 motor[TIRE_BR].dir = FOR; 02592 motor[TIRE_BL].pwm = 20; 02593 motor[TIRE_BL].dir = FOR; 02594 //どれくらい回転するかって値 02595 if(count>=100) { 02596 count=0; 02597 mode=30; 02598 } 02599 } else if(mode==24) { 02600 motor[TIRE_FR].pwm = 10; 02601 motor[TIRE_FR].dir = BRAKE; 02602 motor[TIRE_FL].pwm = 10; 02603 motor[TIRE_FL].dir = BRAKE; 02604 motor[TIRE_BR].pwm = 10; 02605 motor[TIRE_BR].dir = BRAKE; 02606 motor[TIRE_BL].pwm = 10; 02607 motor[TIRE_BL].dir = BRAKE; 02608 if(count>=10000) { 02609 mode=30; 02610 count=0; 02611 } 02612 } 02613 } else { 02614 if(mode==22) { 02615 motor[TIRE_FR].pwm = 0; 02616 motor[TIRE_FR].dir = FREE; 02617 motor[TIRE_FL].pwm = 25; 02618 motor[TIRE_FL].dir = FOR; 02619 motor[TIRE_BR].pwm = 25; 02620 motor[TIRE_BR].dir = BACK; 02621 motor[TIRE_BL].pwm = 0; 02622 motor[TIRE_BL].dir = FREE; 02623 if(g[0]==0&&count>=10000) { 02624 mode=30; 02625 count=0; 02626 cros=0; 02627 } 02628 } 02629 } 02630 if(mode==30) { 02631 motor[TIRE_FR].pwm = 59; 02632 motor[TIRE_FR].dir = BACK; 02633 motor[TIRE_FL].pwm = 62; 02634 motor[TIRE_FL].dir = FOR; 02635 motor[TIRE_BR].pwm = 59; 02636 motor[TIRE_BR].dir = BACK; 02637 motor[TIRE_BL].pwm = 62; 02638 motor[TIRE_BL].dir = FOR; 02639 if(g[0]==98) { 02640 mode=31; 02641 count=0; 02642 } 02643 } else if(mode==31) { 02644 motor[TIRE_FR].pwm = 0; 02645 motor[TIRE_FR].dir = FREE; 02646 motor[TIRE_FL].pwm = 0; 02647 motor[TIRE_FL].dir = FREE; 02648 motor[TIRE_BR].pwm = 0; 02649 motor[TIRE_BR].dir = FREE; 02650 motor[TIRE_BL].pwm = 0; 02651 motor[TIRE_BL].dir = FREE; 02652 if(count>=5000) { 02653 count=0; 02654 mode=32; 02655 } 02656 } else if(mode==32) { 02657 motor[TIRE_FR].pwm = 10; 02658 motor[TIRE_FR].dir = BRAKE; 02659 motor[TIRE_FL].pwm = 10; 02660 motor[TIRE_FL].dir = BRAKE; 02661 motor[TIRE_BR].pwm = 10; 02662 motor[TIRE_BR].dir = BRAKE; 02663 motor[TIRE_BL].pwm = 10; 02664 motor[TIRE_BL].dir = BRAKE; 02665 if(count>=40000) { 02666 mode=33; 02667 count=0; 02668 } 02669 } else if(mode==33) { 02670 motor[TIRE_FR].pwm = 0; 02671 motor[TIRE_FR].dir = BACK; 02672 motor[TIRE_FL].pwm = 30; 02673 motor[TIRE_FL].dir = FOR; 02674 motor[TIRE_BR].pwm = 30; 02675 motor[TIRE_BR].dir = BACK; 02676 motor[TIRE_BL].pwm = 0; 02677 motor[TIRE_BL].dir = FOR; 02678 if(g[3]==0&&count>=20000) { 02679 count=0; 02680 if(Twsh==2) { 02681 solenoid.solenoid2 = SOLENOID_OFF; 02682 } 02683 mode=40; 02684 } 02685 } else if(mode==40) { 02686 if(LIB) { 02687 mode=42; 02688 } 02689 if(counts==false&&g[0]==0) { 02690 cros++; 02691 counts=true; 02692 } 02693 if(counts==true&&g[0]==99) { 02694 counts=false; 02695 } 02696 02697 if(cros==1) { 02698 // mode=41; 02699 //count=0; 02700 //cros=0; 02701 }//ゆっくりモードに入れなかった時の保険 02702 02703 switch(g[3]) { 02704 case 98: 02705 motor[TIRE_FR].pwm = normal; 02706 motor[TIRE_FR].dir = BACK; 02707 motor[TIRE_FL].pwm = normal; 02708 motor[TIRE_FL].dir = BACK; 02709 motor[TIRE_BR].pwm = normal; 02710 motor[TIRE_BR].dir = FOR; 02711 motor[TIRE_BL].pwm = normal; 02712 motor[TIRE_BL].dir = FOR; 02713 if(count>20000) { 02714 cross++; 02715 count=0; 02716 } 02717 mtc2=true; 02718 if(cross==2) { 02719 mode=41; 02720 count=0; 02721 cross=0; 02722 } 02723 break; 02724 case 0: 02725 motor[TIRE_FR].pwm = fast; 02726 motor[TIRE_FR].dir = BACK; 02727 motor[TIRE_FL].pwm = fast; 02728 motor[TIRE_FL].dir = BACK; 02729 motor[TIRE_BR].pwm = fast; 02730 motor[TIRE_BR].dir = FOR; 02731 motor[TIRE_BL].pwm = fast; 02732 motor[TIRE_BL].dir = FOR; 02733 mtc2=true; 02734 break; 02735 case 255: 02736 motor[TIRE_FR].pwm = slow; 02737 motor[TIRE_FR].dir = BACK; 02738 motor[TIRE_FL].pwm = fast; 02739 motor[TIRE_FL].dir = BACK; 02740 motor[TIRE_BR].pwm = fast; 02741 motor[TIRE_BR].dir = FOR; 02742 motor[TIRE_BL].pwm = slow; 02743 motor[TIRE_BL].dir = FOR; 02744 mtc2=true; 02745 break; 02746 case 253: 02747 motor[TIRE_FR].pwm = 0; 02748 motor[TIRE_FR].dir = FREE; 02749 motor[TIRE_FL].pwm = slow; 02750 motor[TIRE_FL].dir = BACK; 02751 motor[TIRE_BR].pwm = slow; 02752 motor[TIRE_BR].dir = FOR; 02753 motor[TIRE_BL].pwm = 0; 02754 motor[TIRE_BL].dir = FREE; 02755 mtc2=true; 02756 break; 02757 case 254: 02758 motor[TIRE_FR].pwm = 0; 02759 motor[TIRE_FR].dir = FREE; 02760 motor[TIRE_FL].pwm = normal; 02761 motor[TIRE_FL].dir = BACK; 02762 motor[TIRE_BR].pwm = normal; 02763 motor[TIRE_BR].dir = FOR; 02764 motor[TIRE_BL].pwm = 0; 02765 motor[TIRE_BL].dir = FREE; 02766 mtc2=true; 02767 break; 02768 case 1: 02769 motor[TIRE_FR].pwm = fast; 02770 motor[TIRE_FR].dir = BACK; 02771 motor[TIRE_FL].pwm = slow; 02772 motor[TIRE_FL].dir = BACK; 02773 motor[TIRE_BR].pwm = slow; 02774 motor[TIRE_BR].dir = FOR; 02775 motor[TIRE_BL].pwm = fast; 02776 motor[TIRE_BL].dir = FOR; 02777 mtc2=true; 02778 break; 02779 case 3: 02780 motor[TIRE_FR].pwm = slow; 02781 motor[TIRE_FR].dir = BACK; 02782 motor[TIRE_FL].pwm = 0; 02783 motor[TIRE_FL].dir = FREE; 02784 motor[TIRE_BR].pwm = 0; 02785 motor[TIRE_BR].dir = FREE; 02786 motor[TIRE_BL].pwm = slow; 02787 motor[TIRE_BL].dir = FOR; 02788 mtc2=true; 02789 break; 02790 case 2: 02791 motor[TIRE_FR].pwm = normal; 02792 motor[TIRE_FR].dir = BACK; 02793 motor[TIRE_FL].pwm = 0; 02794 motor[TIRE_FL].dir = FREE; 02795 motor[TIRE_BR].pwm = 0; 02796 motor[TIRE_BR].dir = FREE; 02797 motor[TIRE_BL].pwm = normal; 02798 motor[TIRE_BL].dir = FOR; 02799 mtc2=true; 02800 break; 02801 } 02802 if(g[2]!=98&&mtc2==true) { 02803 switch(g[2]) { 02804 case 0: 02805 mtc2=false; 02806 break; 02807 case 255: 02808 motor[TIRE_FL].pwm += 5; 02809 mtc2=false; 02810 break; 02811 case 253: 02812 motor[TIRE_FL].pwm += 10; 02813 mtc2=false; 02814 break; 02815 case 254: 02816 motor[TIRE_FL].pwm += 20; 02817 motor[TIRE_BL].pwm = 0; 02818 mtc2=false; 02819 break; 02820 case 1: 02821 motor[TIRE_BL].pwm += 5; 02822 mtc2=false; 02823 break; 02824 case 3: 02825 motor[TIRE_BL].pwm += 10; 02826 mtc2=false; 02827 break; 02828 case 2: 02829 motor[TIRE_BL].pwm += 20; 02830 motor[TIRE_FL].pwm = 0; 02831 mtc2=false; 02832 break; 02833 } 02834 } 02835 } else if(mode==41) { 02836 if(LIF||LIB) { 02837 mode=42; 02838 } 02839 motor[TIRE_FR].pwm = 0; 02840 motor[TIRE_FR].dir = FREE; 02841 motor[TIRE_FL].pwm = 30; 02842 motor[TIRE_FL].dir = BACK; 02843 motor[TIRE_BR].pwm = 30; 02844 motor[TIRE_BR].dir = FOR; 02845 motor[TIRE_BL].pwm = 0; 02846 motor[TIRE_BL].dir = FREE; 02847 if(g[1]==0) { 02848 count=0; 02849 mode=70; 02850 normal=60; 02851 fast=70; 02852 slow=40; 02853 } 02854 } else if(mode==42) { 02855 motor[TIRE_FR].pwm = 20; 02856 motor[TIRE_FR].dir = FOR; 02857 motor[TIRE_FL].pwm = 0; 02858 motor[TIRE_FL].dir = FREE; 02859 motor[TIRE_BR].pwm = 0; 02860 motor[TIRE_BR].dir = FREE; 02861 motor[TIRE_BL].pwm = 20; 02862 motor[TIRE_BL].dir = BACK; 02863 if(g[1]==0) { 02864 count=0; 02865 mode=70; 02866 } 02867 } else if(mode==70) { 02868 switch(g[1]) { 02869 case 99: 02870 if(count>=100000) { 02871 mode=100; 02872 } 02873 break; 02874 case 98: 02875 motor[TIRE_FR].pwm = normal; 02876 motor[TIRE_FR].dir = FOR; 02877 motor[TIRE_FL].pwm = normal; 02878 motor[TIRE_FL].dir = BACK; 02879 motor[TIRE_BR].pwm = normal; 02880 motor[TIRE_BR].dir = FOR; 02881 motor[TIRE_BL].pwm = normal; 02882 motor[TIRE_BL].dir = BACK; 02883 break; 02884 case 0: 02885 motor[TIRE_FR].pwm = 80; 02886 motor[TIRE_FR].dir = FOR; 02887 motor[TIRE_FL].pwm = 80; 02888 motor[TIRE_FL].dir = BACK; 02889 motor[TIRE_BR].pwm = 80; 02890 motor[TIRE_BR].dir = FOR; 02891 motor[TIRE_BL].pwm = 80; 02892 motor[TIRE_BL].dir = BACK; 02893 mtc=true; 02894 break; 02895 case 255: 02896 motor[TIRE_FR].pwm = fast; 02897 motor[TIRE_FR].dir = FOR; 02898 motor[TIRE_FL].pwm = slow; 02899 motor[TIRE_FL].dir = BACK; 02900 motor[TIRE_BR].pwm = slow; 02901 motor[TIRE_BR].dir = FOR; 02902 motor[TIRE_BL].pwm = fast; 02903 motor[TIRE_BL].dir = BACK; 02904 mtc=true; 02905 break; 02906 case 253: 02907 motor[TIRE_FR].pwm = slow; 02908 motor[TIRE_FR].dir = FOR; 02909 motor[TIRE_FL].pwm = 0; 02910 motor[TIRE_FL].dir = FREE; 02911 motor[TIRE_BR].pwm = 0; 02912 motor[TIRE_BR].dir = FREE; 02913 motor[TIRE_BL].pwm = slow; 02914 motor[TIRE_BL].dir = BACK; 02915 mtc=true; 02916 break; 02917 case 254: 02918 motor[TIRE_FR].pwm = normal; 02919 motor[TIRE_FR].dir = FOR; 02920 motor[TIRE_FL].pwm = 0; 02921 motor[TIRE_FL].dir = FREE; 02922 motor[TIRE_BR].pwm = 0; 02923 motor[TIRE_BR].dir = FREE; 02924 motor[TIRE_BL].pwm = normal; 02925 motor[TIRE_BL].dir = BACK; 02926 mtc=true; 02927 break; 02928 case 1: 02929 motor[TIRE_FR].pwm = slow; 02930 motor[TIRE_FR].dir = FOR; 02931 motor[TIRE_FL].pwm = fast; 02932 motor[TIRE_FL].dir = BACK; 02933 motor[TIRE_BR].pwm = fast; 02934 motor[TIRE_BR].dir = FOR; 02935 motor[TIRE_BL].pwm = slow; 02936 motor[TIRE_BL].dir = BACK; 02937 mtc=true; 02938 break; 02939 case 3: 02940 motor[TIRE_FR].pwm = 0; 02941 motor[TIRE_FR].dir = FREE; 02942 motor[TIRE_FL].pwm = slow; 02943 motor[TIRE_FL].dir = BACK; 02944 motor[TIRE_BR].pwm = slow; 02945 motor[TIRE_BR].dir = FOR; 02946 motor[TIRE_BL].pwm = 0; 02947 motor[TIRE_BL].dir = FREE; 02948 mtc=true; 02949 break; 02950 case 2: 02951 motor[TIRE_FR].pwm = 0; 02952 motor[TIRE_FR].dir = FREE; 02953 motor[TIRE_FL].pwm = normal; 02954 motor[TIRE_FL].dir = BACK; 02955 motor[TIRE_BR].pwm = normal; 02956 motor[TIRE_BR].dir = FOR; 02957 motor[TIRE_BL].pwm = 0; 02958 motor[TIRE_BL].dir = FREE; 02959 mtc=true; 02960 break; 02961 } 02962 if(g[1]!=98&&mtc==true) { 02963 switch(g[0]) { 02964 case 0: 02965 mtc=false; 02966 break; 02967 case 255: 02968 motor[TIRE_FL].pwm += 5; 02969 mtc=false; 02970 break; 02971 case 253: 02972 motor[TIRE_FL].pwm += 10; 02973 mtc=false; 02974 break; 02975 case 254: 02976 motor[TIRE_FL].pwm += 20; 02977 motor[TIRE_FR].pwm = 0; 02978 mtc=false; 02979 break; 02980 case 1: 02981 motor[TIRE_FR].pwm += 5; 02982 mtc=false; 02983 break; 02984 case 3: 02985 motor[TIRE_FR].pwm += 10; 02986 mtc=false; 02987 break; 02988 case 2: 02989 motor[TIRE_FR].pwm += 20; 02990 motor[TIRE_FL].pwm = 0; 02991 mtc=false; 02992 break; 02993 } 02994 } 02995 } else if(mode==100) { 02996 motor[TIRE_FR].pwm = 20; 02997 motor[TIRE_FR].dir = FOR; 02998 motor[TIRE_FL].pwm = 20; 02999 motor[TIRE_FL].dir = BACK; 03000 motor[TIRE_BR].pwm = 23; 03001 motor[TIRE_BR].dir = FOR; 03002 motor[TIRE_BL].pwm = 20; 03003 motor[TIRE_BL].dir = BACK; 03004 } else { 03005 } 03006 } 03007 #endif 03008 03009 #if USE_PROCESS_NUM>7 03010 static void Process7() 03011 { 03012 03013 } 03014 #endif 03015 03016 #if USE_PROCESS_NUM>8 03017 static void Process8() 03018 { 03019 03020 } 03021 #endif 03022 03023 #if USE_PROCESS_NUM>9 03024 static void Process9() 03025 { 03026 03027 } 03028 #endif 03029 #endif 03030 #pragma endregion PROCESS 03031 03032 static void AllActuatorReset() 03033 { 03034 03035 #ifdef USE_SOLENOID 03036 solenoid.all = ALL_SOLENOID_OFF; 03037 #endif 03038 03039 #ifdef USE_MOTOR 03040 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 03041 motor[i].dir = FREE; 03042 motor[i].pwm = 0; 03043 } 03044 #endif 03045 } 03046 03047 /* 03048 void BuzzerTimer_func() 03049 { 03050 buzzer = !buzzer; 03051 //LED_DEBUG0 = !LED_DEBUG0; 03052 } 03053 */ 03054 void TapeLedEms_func() 03055 { 03056 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 03057 } 03058 03059 #pragma region USER-DEFINED-FUNCTIONS 03060 03061 03062 #pragma endregion
Generated on Thu Jul 14 2022 23:08:50 by
1.7.2