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