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