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