daad
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 static void Process0() 00274 { 00275 if(StertSW) { 00276 if(ZoneSW) { 00277 current = 1; 00278 Twsh=2; 00279 } else { 00280 current = 1; 00281 Twsh=3; 00282 } 00283 } 00284 } 00285 #endif 00286 00287 #if USE_PROCESS_NUM>1 00288 00289 00290 bool dz1=true; 00291 bool dz1i=false; 00292 bool dz2=true; 00293 bool dz2i=false; 00294 bool dz3=true; 00295 bool dz3i=false; 00296 bool dz4=true; 00297 bool dz4i=false; 00298 00299 int mode=10; 00300 int mode1=0; 00301 int mode1g0=100000; 00302 00303 00304 static void Process1() 00305 { 00306 mode1g0=100000; 00307 mode=1; 00308 mode1=0; 00309 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00310 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00311 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00312 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00313 00314 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00315 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00316 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00317 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00318 00319 if(controller->Button.UP||controller->Button.DOWN) { 00320 00321 if(controller->Button.UP) { 00322 motor[MOTOR_0].pwm = 110; 00323 motor[MOTOR_0].dir = FOR; 00324 } 00325 if(controller->Button.DOWN) { 00326 motor[MOTOR_0].pwm = 50; 00327 motor[MOTOR_0].dir = BACK; 00328 } 00329 } else { 00330 motor[MOTOR_0].pwm = 0; 00331 motor[MOTOR_0].dir = BRAKE; 00332 } 00333 if(controller->Button.X) { 00334 if(dz1==true) { 00335 if(dz1i==false) { 00336 solenoid.solenoid1 = SOLENOID_ON; 00337 dz1i=true; 00338 } else { 00339 solenoid.solenoid1 = SOLENOID_OFF; 00340 dz1i=false; 00341 } 00342 dz1=false; 00343 } 00344 } else { 00345 dz1=true; 00346 } 00347 00348 if(controller->Button.Y) { 00349 if(dz2==true) { 00350 if(dz2i==false) { 00351 solenoid.solenoid2 = SOLENOID_ON; 00352 dz2i=true; 00353 } else { 00354 solenoid.solenoid2 = SOLENOID_OFF; 00355 dz2i=false; 00356 } 00357 dz2=false; 00358 } 00359 } else { 00360 dz2=true; 00361 } 00362 00363 if(controller->Button.A) { 00364 if(dz3==true) { 00365 if(dz3i==false) { 00366 solenoid.solenoid3 = SOLENOID_ON; 00367 dz3i=true; 00368 } else { 00369 solenoid.solenoid3 = SOLENOID_OFF; 00370 dz3i=false; 00371 } 00372 dz3=false; 00373 } 00374 } else { 00375 dz3=true; 00376 } 00377 00378 if(controller->Button.B) { 00379 if(dz4==true) { 00380 if(dz4i==false) { 00381 solenoid.solenoid4 = SOLENOID_ON; 00382 dz4i=true; 00383 } else { 00384 solenoid.solenoid4 = SOLENOID_OFF; 00385 dz4i=false; 00386 } 00387 dz4=false; 00388 } 00389 } else { 00390 dz4=true; 00391 } 00392 /* 00393 if(controller->Button.RIGHT){ 00394 motor[MOTOR_1].dir = FOR; 00395 motor[MOTOR_1].pwm = 60; 00396 if (ARM_1){ 00397 motor[MOTOR_1].dir = BRAKE; 00398 } 00399 } 00400 else if(controller->Button.LEFT){ 00401 motor[MOTOR_1].dir = BACK; 00402 motor[MOTOR_1].pwm = 60; 00403 if (ARM_0){ 00404 motor[MOTOR_1].dir = BRAKE; 00405 } 00406 } 00407 */ 00408 } 00409 #endif 00410 00411 #if USE_PROCESS_NUM>2 00412 bool mtc=false; 00413 //追加 00414 bool mtc2 = false; 00415 00416 static void Process2() 00417 { 00418 // 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]); 00419 mode1g0++; 00420 if(mode==1) { 00421 motor[TIRE_FR].pwm = 15; 00422 motor[TIRE_FR].dir = BACK; 00423 motor[TIRE_FL].pwm = 15; 00424 motor[TIRE_FL].dir = FOR; 00425 motor[TIRE_BR].pwm = 15; 00426 motor[TIRE_BR].dir = BACK; 00427 motor[TIRE_BL].pwm = 15; 00428 motor[TIRE_BL].dir = FOR; 00429 if(mode1g0>5000&&g[2]==0) { 00430 mode1g0=0; 00431 mode=2; 00432 } 00433 } else if(mode==2) { 00434 switch(g[2]) { 00435 case 98: 00436 motor[TIRE_FR].pwm = 30; 00437 motor[TIRE_FR].dir = FOR; 00438 motor[TIRE_FL].pwm = 30; 00439 motor[TIRE_FL].dir = FOR; 00440 motor[TIRE_BR].pwm = 30; 00441 motor[TIRE_BR].dir = BACK; 00442 motor[TIRE_BL].pwm = 30; 00443 motor[TIRE_BL].dir = BACK; 00444 if(mode1g0>100000) { 00445 mode1++; 00446 mode1g0=0; 00447 } 00448 mtc2=true; 00449 if(mode1==1) { 00450 mode=3; 00451 mode1g0=0; 00452 mode1=0; 00453 } 00454 break; 00455 case 0: 00456 motor[TIRE_FR].pwm = 60; 00457 motor[TIRE_FR].dir = FOR; 00458 motor[TIRE_FL].pwm = 60; 00459 motor[TIRE_FL].dir = FOR; 00460 motor[TIRE_BR].pwm = 60; 00461 motor[TIRE_BR].dir = BACK; 00462 motor[TIRE_BL].pwm = 60; 00463 motor[TIRE_BL].dir = BACK; 00464 mtc2=true; 00465 break; 00466 case 255: 00467 motor[TIRE_FR].pwm = 40; 00468 motor[TIRE_FR].dir = FOR; 00469 motor[TIRE_FL].pwm = 60; 00470 motor[TIRE_FL].dir = FOR; 00471 motor[TIRE_BR].pwm = 60; 00472 motor[TIRE_BR].dir = BACK; 00473 motor[TIRE_BL].pwm = 40; 00474 motor[TIRE_BL].dir = BACK; 00475 mtc2=true; 00476 break; 00477 case 253: 00478 motor[TIRE_FR].pwm = 0; 00479 motor[TIRE_FR].dir = FREE; 00480 motor[TIRE_FL].pwm = 20; 00481 motor[TIRE_FL].dir = FOR; 00482 motor[TIRE_BR].pwm = 20; 00483 motor[TIRE_BR].dir = BACK; 00484 motor[TIRE_BL].pwm = 0; 00485 motor[TIRE_BL].dir = FREE; 00486 mtc2=true; 00487 break; 00488 case 254: 00489 motor[TIRE_FR].pwm = 0; 00490 motor[TIRE_FR].dir = FREE; 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 = 0; 00496 motor[TIRE_BL].dir = FREE; 00497 mtc2=true; 00498 break; 00499 case 1: 00500 motor[TIRE_FR].pwm = 60; 00501 motor[TIRE_FR].dir = FOR; 00502 motor[TIRE_FL].pwm = 40; 00503 motor[TIRE_FL].dir = FOR; 00504 motor[TIRE_BR].pwm = 40; 00505 motor[TIRE_BR].dir = BACK; 00506 motor[TIRE_BL].pwm = 60; 00507 motor[TIRE_BL].dir = BACK; 00508 mtc2=true; 00509 break; 00510 case 3: 00511 motor[TIRE_FR].pwm = 20; 00512 motor[TIRE_FR].dir = FOR; 00513 motor[TIRE_FL].pwm = 0; 00514 motor[TIRE_FL].dir = FREE; 00515 motor[TIRE_BR].pwm = 0; 00516 motor[TIRE_BR].dir = FREE; 00517 motor[TIRE_BL].pwm = 20; 00518 motor[TIRE_BL].dir = BACK; 00519 mtc2=true; 00520 break; 00521 case 2: 00522 motor[TIRE_FR].pwm = 30; 00523 motor[TIRE_FR].dir = FOR; 00524 motor[TIRE_FL].pwm = 0; 00525 motor[TIRE_FL].dir = FREE; 00526 motor[TIRE_BR].pwm = 0; 00527 motor[TIRE_BR].dir = FREE; 00528 motor[TIRE_BL].pwm = 30; 00529 motor[TIRE_BL].dir = BACK; 00530 mtc2=true; 00531 break; 00532 } 00533 if(g[2]!=98&&mtc2==true) { 00534 switch(g[3]) { 00535 case 0: 00536 mtc2=false; 00537 break; 00538 case 255: 00539 motor[TIRE_BL].pwm += 5; 00540 mtc2=false; 00541 break; 00542 case 253: 00543 motor[TIRE_BL].pwm += 10; 00544 mtc2=false; 00545 break; 00546 case 254: 00547 motor[TIRE_BL].pwm += 20; 00548 motor[TIRE_FL].pwm = 0; 00549 mtc2=false; 00550 break; 00551 case 1: 00552 motor[TIRE_FL].pwm += 5; 00553 mtc2=false; 00554 break; 00555 case 3: 00556 motor[TIRE_FL].pwm += 10; 00557 mtc2=false; 00558 break; 00559 case 2: 00560 motor[TIRE_FL].pwm += 20; 00561 motor[TIRE_BL].pwm = 0; 00562 mtc2=false; 00563 break; 00564 } 00565 } 00566 } else if(mode==3) { 00567 motor[TIRE_FR].pwm = 0; 00568 motor[TIRE_FR].dir = FREE; 00569 motor[TIRE_FL].pwm = 15; 00570 motor[TIRE_FL].dir = FOR; 00571 motor[TIRE_BR].pwm = 15; 00572 motor[TIRE_BR].dir = BACK; 00573 motor[TIRE_BL].pwm = 0; 00574 motor[TIRE_BL].dir = FREE; 00575 if(mode1g0>5000&&g[0]==0) { 00576 mode1g0=0; 00577 mode=4; 00578 } 00579 } else if(mode==4) { 00580 motor[TIRE_FR].pwm = 15; 00581 motor[TIRE_FR].dir = BACK; 00582 motor[TIRE_FL].pwm = 15; 00583 motor[TIRE_FL].dir = BACK; 00584 motor[TIRE_BR].pwm = 15; 00585 motor[TIRE_BR].dir = FOR; 00586 motor[TIRE_BL].pwm = 15; 00587 motor[TIRE_BL].dir = FOR; 00588 if(mode1g0>10000&&g[0]==0) { 00589 mode1g0=0; 00590 mode1=0; 00591 mode=10; 00592 } 00593 } else if(g[0]!=99&&mode==10) { 00594 switch(g[0]) { 00595 case 98: 00596 motor[TIRE_FR].pwm = 30; 00597 motor[TIRE_FR].dir = BACK; 00598 motor[TIRE_FL].pwm = 30; 00599 motor[TIRE_FL].dir = FOR; 00600 motor[TIRE_BR].pwm = 30; 00601 motor[TIRE_BR].dir = BACK; 00602 motor[TIRE_BL].pwm = 30; 00603 motor[TIRE_BL].dir = FOR; 00604 if(mode1g0>100000) { 00605 mode1++; 00606 mode1g0=0; 00607 } 00608 mtc=true; 00609 if(mode1==Twsh) { 00610 mode=11; 00611 mode1g0=0; 00612 } 00613 break; 00614 case 0: 00615 motor[TIRE_FR].pwm = 60; 00616 motor[TIRE_FR].dir = BACK; 00617 motor[TIRE_FL].pwm = 60; 00618 motor[TIRE_FL].dir = FOR; 00619 motor[TIRE_BR].pwm = 60; 00620 motor[TIRE_BR].dir = BACK; 00621 motor[TIRE_BL].pwm = 60; 00622 motor[TIRE_BL].dir = FOR; 00623 mtc=true; 00624 break; 00625 case 255: 00626 motor[TIRE_FR].pwm = 60; 00627 motor[TIRE_FR].dir = BACK; 00628 motor[TIRE_FL].pwm = 40; 00629 motor[TIRE_FL].dir = FOR; 00630 motor[TIRE_BR].pwm = 40; 00631 motor[TIRE_BR].dir = BACK; 00632 motor[TIRE_BL].pwm = 60; 00633 motor[TIRE_BL].dir = FOR; 00634 mtc=true; 00635 break; 00636 case 253: 00637 motor[TIRE_FR].pwm = 20; 00638 motor[TIRE_FR].dir = BACK; 00639 motor[TIRE_FL].pwm = 0; 00640 motor[TIRE_FL].dir = FREE; 00641 motor[TIRE_BR].pwm = 0; 00642 motor[TIRE_BR].dir = FREE; 00643 motor[TIRE_BL].pwm = 20; 00644 motor[TIRE_BL].dir = FOR; 00645 mtc=true; 00646 break; 00647 case 254: 00648 motor[TIRE_FR].pwm = 30; 00649 motor[TIRE_FR].dir = BACK; 00650 motor[TIRE_FL].pwm = 0; 00651 motor[TIRE_FL].dir = FREE; 00652 motor[TIRE_BR].pwm = 0; 00653 motor[TIRE_BR].dir = FREE; 00654 motor[TIRE_BL].pwm = 30; 00655 motor[TIRE_BL].dir = FOR; 00656 mtc=true; 00657 break; 00658 case 1: 00659 motor[TIRE_FR].pwm = 40; 00660 motor[TIRE_FR].dir = BACK; 00661 motor[TIRE_FL].pwm = 60; 00662 motor[TIRE_FL].dir = FOR; 00663 motor[TIRE_BR].pwm = 60; 00664 motor[TIRE_BR].dir = BACK; 00665 motor[TIRE_BL].pwm = 40; 00666 motor[TIRE_BL].dir = FOR; 00667 mtc=true; 00668 break; 00669 case 3: 00670 motor[TIRE_FR].pwm = 0; 00671 motor[TIRE_FR].dir = FREE; 00672 motor[TIRE_FL].pwm = 20; 00673 motor[TIRE_FL].dir = FOR; 00674 motor[TIRE_BR].pwm = 20; 00675 motor[TIRE_BR].dir = BACK; 00676 motor[TIRE_BL].pwm = 0; 00677 motor[TIRE_BL].dir = FREE; 00678 mtc=true; 00679 break; 00680 case 2: 00681 motor[TIRE_FR].pwm = 0; 00682 motor[TIRE_FR].dir = FREE; 00683 motor[TIRE_FL].pwm = 30; 00684 motor[TIRE_FL].dir = FOR; 00685 motor[TIRE_BR].pwm = 30; 00686 motor[TIRE_BR].dir = BACK; 00687 motor[TIRE_BL].pwm = 0; 00688 motor[TIRE_BL].dir = FREE; 00689 mtc=true; 00690 break; 00691 } 00692 if(g[0]!=98&&mtc==true) { 00693 switch(g[1]) { 00694 case 0: 00695 mtc=false; 00696 break; 00697 case 255: 00698 motor[TIRE_BR].pwm += 5; 00699 mtc=false; 00700 break; 00701 case 253: 00702 motor[TIRE_BR].pwm += 10; 00703 mtc=false; 00704 break; 00705 case 254: 00706 motor[TIRE_BR].pwm += 20; 00707 motor[TIRE_BL].pwm = 0; 00708 mtc=false; 00709 break; 00710 case 1: 00711 motor[TIRE_BL].pwm += 5; 00712 mtc=false; 00713 break; 00714 case 3: 00715 motor[TIRE_BL].pwm += 10; 00716 mtc=false; 00717 break; 00718 case 2: 00719 motor[TIRE_BL].pwm += 20; 00720 motor[TIRE_BR].pwm = 0; 00721 mtc=false; 00722 break; 00723 } 00724 } 00725 } else if(mode==11) { 00726 motor[TIRE_FR].pwm = 0; 00727 motor[TIRE_FR].dir = FREE; 00728 motor[TIRE_FL].pwm = 15; 00729 motor[TIRE_FL].dir = FOR; 00730 motor[TIRE_BR].pwm = 15; 00731 motor[TIRE_BR].dir = BACK; 00732 motor[TIRE_BL].pwm = 0; 00733 motor[TIRE_BL].dir = FREE; 00734 if(mode1g0>5000&&g[2]==0) { 00735 mode1g0=0; 00736 mode=12; 00737 } 00738 } else if(mode==12) { 00739 motor[TIRE_FR].pwm = 15; 00740 motor[TIRE_FR].dir = FOR; 00741 motor[TIRE_FL].pwm = 15; 00742 motor[TIRE_FL].dir = BACK; 00743 motor[TIRE_BR].pwm = 15; 00744 motor[TIRE_BR].dir = FOR; 00745 motor[TIRE_BL].pwm = 15; 00746 motor[TIRE_BL].dir = BACK; 00747 mode1g0++; 00748 if(mode1g0>10000&&g[2]==0) { 00749 mode1g0=0; 00750 mode1=0; 00751 mode=20; 00752 } 00753 } else if(mode==20) { 00754 switch(g[2]) { 00755 case 98: 00756 motor[TIRE_FR].pwm = 30; 00757 motor[TIRE_FR].dir = FOR; 00758 motor[TIRE_FL].pwm = 30; 00759 motor[TIRE_FL].dir = FOR; 00760 motor[TIRE_BR].pwm = 30; 00761 motor[TIRE_BR].dir = BACK; 00762 motor[TIRE_BL].pwm = 30; 00763 motor[TIRE_BL].dir = BACK; 00764 if(mode1g0>100000) { 00765 mode1++; 00766 mode1g0=0; 00767 } 00768 mtc2=true; 00769 if(mode1==3) { 00770 mode=21; 00771 mode1g0=0; 00772 mode1=0; 00773 } 00774 break; 00775 case 0: 00776 motor[TIRE_FR].pwm = 60; 00777 motor[TIRE_FR].dir = FOR; 00778 motor[TIRE_FL].pwm = 60; 00779 motor[TIRE_FL].dir = FOR; 00780 motor[TIRE_BR].pwm = 60; 00781 motor[TIRE_BR].dir = BACK; 00782 motor[TIRE_BL].pwm = 60; 00783 motor[TIRE_BL].dir = BACK; 00784 mtc2=true; 00785 break; 00786 case 255: 00787 motor[TIRE_FR].pwm = 40; 00788 motor[TIRE_FR].dir = FOR; 00789 motor[TIRE_FL].pwm = 60; 00790 motor[TIRE_FL].dir = FOR; 00791 motor[TIRE_BR].pwm = 60; 00792 motor[TIRE_BR].dir = BACK; 00793 motor[TIRE_BL].pwm = 40; 00794 motor[TIRE_BL].dir = BACK; 00795 mtc2=true; 00796 break; 00797 case 253: 00798 motor[TIRE_FR].pwm = 0; 00799 motor[TIRE_FR].dir = FREE; 00800 motor[TIRE_FL].pwm = 20; 00801 motor[TIRE_FL].dir = FOR; 00802 motor[TIRE_BR].pwm = 20; 00803 motor[TIRE_BR].dir = BACK; 00804 motor[TIRE_BL].pwm = 0; 00805 motor[TIRE_BL].dir = FREE; 00806 mtc2=true; 00807 break; 00808 case 254: 00809 motor[TIRE_FR].pwm = 0; 00810 motor[TIRE_FR].dir = FREE; 00811 motor[TIRE_FL].pwm = 30; 00812 motor[TIRE_FL].dir = FOR; 00813 motor[TIRE_BR].pwm = 30; 00814 motor[TIRE_BR].dir = BACK; 00815 motor[TIRE_BL].pwm = 0; 00816 motor[TIRE_BL].dir = FREE; 00817 mtc2=true; 00818 break; 00819 case 1: 00820 motor[TIRE_FR].pwm = 60; 00821 motor[TIRE_FR].dir = FOR; 00822 motor[TIRE_FL].pwm = 40; 00823 motor[TIRE_FL].dir = FOR; 00824 motor[TIRE_BR].pwm = 40; 00825 motor[TIRE_BR].dir = BACK; 00826 motor[TIRE_BL].pwm = 60; 00827 motor[TIRE_BL].dir = BACK; 00828 mtc2=true; 00829 break; 00830 case 3: 00831 motor[TIRE_FR].pwm = 20; 00832 motor[TIRE_FR].dir = FOR; 00833 motor[TIRE_FL].pwm = 0; 00834 motor[TIRE_FL].dir = FREE; 00835 motor[TIRE_BR].pwm = 0; 00836 motor[TIRE_BR].dir = FREE; 00837 motor[TIRE_BL].pwm = 20; 00838 motor[TIRE_BL].dir = BACK; 00839 mtc2=true; 00840 break; 00841 case 2: 00842 motor[TIRE_FR].pwm = 30; 00843 motor[TIRE_FR].dir = FOR; 00844 motor[TIRE_FL].pwm = 0; 00845 motor[TIRE_FL].dir = FREE; 00846 motor[TIRE_BR].pwm = 0; 00847 motor[TIRE_BR].dir = FREE; 00848 motor[TIRE_BL].pwm = 30; 00849 motor[TIRE_BL].dir = BACK; 00850 mtc2=true; 00851 break; 00852 } 00853 if(g[2]!=98&&mtc2==true) { 00854 switch(g[3]) { 00855 case 0: 00856 mtc2=false; 00857 break; 00858 case 255: 00859 motor[TIRE_BL].pwm += 5; 00860 mtc2=false; 00861 break; 00862 case 253: 00863 motor[TIRE_BL].pwm += 10; 00864 mtc2=false; 00865 break; 00866 case 254: 00867 motor[TIRE_BL].pwm += 20; 00868 motor[TIRE_FL].pwm = 0; 00869 mtc2=false; 00870 break; 00871 case 1: 00872 motor[TIRE_FL].pwm += 5; 00873 mtc2=false; 00874 break; 00875 case 3: 00876 motor[TIRE_FL].pwm += 10; 00877 mtc2=false; 00878 break; 00879 case 2: 00880 motor[TIRE_FL].pwm += 20; 00881 motor[TIRE_BL].pwm = 0; 00882 mtc2=false; 00883 break; 00884 } 00885 } 00886 } else if(mode==21) { 00887 motor[TIRE_FR].pwm = 0; 00888 motor[TIRE_FR].dir = FREE; 00889 motor[TIRE_FL].pwm = 15; 00890 motor[TIRE_FL].dir = FOR; 00891 motor[TIRE_BR].pwm = 15; 00892 motor[TIRE_BR].dir = BACK; 00893 motor[TIRE_BL].pwm = 0; 00894 motor[TIRE_BL].dir = FREE; 00895 if(mode1g0>5000&&g[0]==0) { 00896 mode1g0=0; 00897 mode=22; 00898 } 00899 } else if(mode==22) { 00900 motor[TIRE_FR].pwm = 15; 00901 motor[TIRE_FR].dir = BACK; 00902 motor[TIRE_FL].pwm = 15; 00903 motor[TIRE_FL].dir = BACK; 00904 motor[TIRE_BR].pwm = 15; 00905 motor[TIRE_BR].dir = FOR; 00906 motor[TIRE_BL].pwm = 15; 00907 motor[TIRE_BL].dir = FOR; 00908 if(mode1g0>10000&&g[0]==0) { 00909 mode1g0=0; 00910 mode1=0; 00911 mode=30; 00912 } 00913 } else if(mode==30) { 00914 switch(g[0]) { 00915 case 98: 00916 motor[TIRE_FR].pwm = 30; 00917 motor[TIRE_FR].dir = BACK; 00918 motor[TIRE_FL].pwm = 30; 00919 motor[TIRE_FL].dir = FOR; 00920 motor[TIRE_BR].pwm = 30; 00921 motor[TIRE_BR].dir = BACK; 00922 motor[TIRE_BL].pwm = 30; 00923 motor[TIRE_BL].dir = FOR; 00924 if(mode1g0>100000) { 00925 mode1++; 00926 mode1g0=0; 00927 } 00928 mtc=true; 00929 if(mode1==1) { 00930 mode=31; 00931 mode1g0=0; 00932 } 00933 break; 00934 case 0: 00935 motor[TIRE_FR].pwm = 60; 00936 motor[TIRE_FR].dir = BACK; 00937 motor[TIRE_FL].pwm = 60; 00938 motor[TIRE_FL].dir = FOR; 00939 motor[TIRE_BR].pwm = 60; 00940 motor[TIRE_BR].dir = BACK; 00941 motor[TIRE_BL].pwm = 60; 00942 motor[TIRE_BL].dir = FOR; 00943 mtc=true; 00944 break; 00945 case 255: 00946 motor[TIRE_FR].pwm = 60; 00947 motor[TIRE_FR].dir = BACK; 00948 motor[TIRE_FL].pwm = 40; 00949 motor[TIRE_FL].dir = FOR; 00950 motor[TIRE_BR].pwm = 40; 00951 motor[TIRE_BR].dir = BACK; 00952 motor[TIRE_BL].pwm = 60; 00953 motor[TIRE_BL].dir = FOR; 00954 mtc=true; 00955 break; 00956 case 253: 00957 motor[TIRE_FR].pwm = 20; 00958 motor[TIRE_FR].dir = BACK; 00959 motor[TIRE_FL].pwm = 0; 00960 motor[TIRE_FL].dir = FREE; 00961 motor[TIRE_BR].pwm = 0; 00962 motor[TIRE_BR].dir = FREE; 00963 motor[TIRE_BL].pwm = 20; 00964 motor[TIRE_BL].dir = FOR; 00965 mtc=true; 00966 break; 00967 case 254: 00968 motor[TIRE_FR].pwm = 30; 00969 motor[TIRE_FR].dir = BACK; 00970 motor[TIRE_FL].pwm = 0; 00971 motor[TIRE_FL].dir = FREE; 00972 motor[TIRE_BR].pwm = 0; 00973 motor[TIRE_BR].dir = FREE; 00974 motor[TIRE_BL].pwm = 30; 00975 motor[TIRE_BL].dir = FOR; 00976 mtc=true; 00977 break; 00978 case 1: 00979 motor[TIRE_FR].pwm = 40; 00980 motor[TIRE_FR].dir = BACK; 00981 motor[TIRE_FL].pwm = 60; 00982 motor[TIRE_FL].dir = FOR; 00983 motor[TIRE_BR].pwm = 60; 00984 motor[TIRE_BR].dir = BACK; 00985 motor[TIRE_BL].pwm = 40; 00986 motor[TIRE_BL].dir = FOR; 00987 mtc=true; 00988 break; 00989 case 3: 00990 motor[TIRE_FR].pwm = 0; 00991 motor[TIRE_FR].dir = FREE; 00992 motor[TIRE_FL].pwm = 20; 00993 motor[TIRE_FL].dir = FOR; 00994 motor[TIRE_BR].pwm = 20; 00995 motor[TIRE_BR].dir = BACK; 00996 motor[TIRE_BL].pwm = 0; 00997 motor[TIRE_BL].dir = FREE; 00998 mtc=true; 00999 break; 01000 case 2: 01001 motor[TIRE_FR].pwm = 0; 01002 motor[TIRE_FR].dir = FREE; 01003 motor[TIRE_FL].pwm = 30; 01004 motor[TIRE_FL].dir = FOR; 01005 motor[TIRE_BR].pwm = 30; 01006 motor[TIRE_BR].dir = BACK; 01007 motor[TIRE_BL].pwm = 0; 01008 motor[TIRE_BL].dir = FREE; 01009 mtc=true; 01010 break; 01011 } 01012 if(g[0]!=98&&mtc==true) { 01013 switch(g[1]) { 01014 case 0: 01015 mtc=false; 01016 break; 01017 case 255: 01018 motor[TIRE_BR].pwm += 5; 01019 mtc=false; 01020 break; 01021 case 253: 01022 motor[TIRE_BR].pwm += 10; 01023 mtc=false; 01024 break; 01025 case 254: 01026 motor[TIRE_BR].pwm += 20; 01027 motor[TIRE_BL].pwm = 0; 01028 mtc=false; 01029 break; 01030 case 1: 01031 motor[TIRE_BL].pwm += 5; 01032 mtc=false; 01033 break; 01034 case 3: 01035 motor[TIRE_BL].pwm += 10; 01036 mtc=false; 01037 break; 01038 case 2: 01039 motor[TIRE_BL].pwm += 20; 01040 motor[TIRE_BR].pwm = 0; 01041 mtc=false; 01042 break; 01043 } 01044 } 01045 } else if(mode==31) { 01046 motor[TIRE_FR].pwm = 15; 01047 motor[TIRE_FR].dir = BACK; 01048 motor[TIRE_FL].pwm = 0; 01049 motor[TIRE_FL].dir = FREE; 01050 motor[TIRE_BR].pwm = 0; 01051 motor[TIRE_BR].dir = FREE; 01052 motor[TIRE_BL].pwm = 15; 01053 motor[TIRE_BL].dir = FOR; 01054 mode1g0++; 01055 if(mode1g0>5000&&g[3]==0) { 01056 mode1g0=0; 01057 mode=32; 01058 } 01059 } else if(mode==32) { 01060 motor[TIRE_FR].pwm = 15; 01061 motor[TIRE_FR].dir = FOR; 01062 motor[TIRE_FL].pwm = 15; 01063 motor[TIRE_FL].dir = BACK; 01064 motor[TIRE_BR].pwm = 15; 01065 motor[TIRE_BR].dir = FOR; 01066 motor[TIRE_BL].pwm = 15; 01067 motor[TIRE_BL].dir = BACK; 01068 mode1g0++; 01069 if(mode1g0>5000&&g[3]==0) { 01070 mode1g0=0; 01071 mode1=0; 01072 mode=40; 01073 } 01074 } else if(mode==40) { 01075 switch(g[3]) { 01076 case 98: 01077 motor[TIRE_FR].pwm = 30; 01078 motor[TIRE_FR].dir = BACK; 01079 motor[TIRE_FL].pwm = 30; 01080 motor[TIRE_FL].dir = BACK; 01081 motor[TIRE_BR].pwm = 30; 01082 motor[TIRE_BR].dir = FOR; 01083 motor[TIRE_BL].pwm = 30; 01084 motor[TIRE_BL].dir = FOR; 01085 if(mode1g0>100000) { 01086 mode1++; 01087 mode1g0=0; 01088 } 01089 mtc2=true; 01090 if(mode1==1) { 01091 mode=41; 01092 mode1g0=0; 01093 mode1=0; 01094 } 01095 break; 01096 case 0: 01097 motor[TIRE_FR].pwm = 60; 01098 motor[TIRE_FR].dir = BACK; 01099 motor[TIRE_FL].pwm = 60; 01100 motor[TIRE_FL].dir = BACK; 01101 motor[TIRE_BR].pwm = 60; 01102 motor[TIRE_BR].dir = FOR; 01103 motor[TIRE_BL].pwm = 60; 01104 motor[TIRE_BL].dir = FOR; 01105 mtc2=true; 01106 break; 01107 case 255: 01108 motor[TIRE_FR].pwm = 40; 01109 motor[TIRE_FR].dir = BACK; 01110 motor[TIRE_FL].pwm = 60; 01111 motor[TIRE_FL].dir = BACK; 01112 motor[TIRE_BR].pwm = 60; 01113 motor[TIRE_BR].dir = FOR; 01114 motor[TIRE_BL].pwm = 40; 01115 motor[TIRE_BL].dir = FOR; 01116 mtc2=true; 01117 break; 01118 case 253: 01119 motor[TIRE_FR].pwm = 0; 01120 motor[TIRE_FR].dir = FREE; 01121 motor[TIRE_FL].pwm = 20; 01122 motor[TIRE_FL].dir = BACK; 01123 motor[TIRE_BR].pwm = 20; 01124 motor[TIRE_BR].dir = FOR; 01125 motor[TIRE_BL].pwm = 0; 01126 motor[TIRE_BL].dir = FREE; 01127 mtc2=true; 01128 break; 01129 case 254: 01130 motor[TIRE_FR].pwm = 0; 01131 motor[TIRE_FR].dir = FREE; 01132 motor[TIRE_FL].pwm = 30; 01133 motor[TIRE_FL].dir = BACK; 01134 motor[TIRE_BR].pwm = 30; 01135 motor[TIRE_BR].dir = FOR; 01136 motor[TIRE_BL].pwm = 0; 01137 motor[TIRE_BL].dir = FREE; 01138 mtc2=true; 01139 break; 01140 case 1: 01141 motor[TIRE_FR].pwm = 60; 01142 motor[TIRE_FR].dir = BACK; 01143 motor[TIRE_FL].pwm = 40; 01144 motor[TIRE_FL].dir = BACK; 01145 motor[TIRE_BR].pwm = 40; 01146 motor[TIRE_BR].dir = FOR; 01147 motor[TIRE_BL].pwm = 60; 01148 motor[TIRE_BL].dir = FOR; 01149 mtc2=true; 01150 break; 01151 case 3: 01152 motor[TIRE_FR].pwm = 20; 01153 motor[TIRE_FR].dir = BACK; 01154 motor[TIRE_FL].pwm = 0; 01155 motor[TIRE_FL].dir = FREE; 01156 motor[TIRE_BR].pwm = 0; 01157 motor[TIRE_BR].dir = FREE; 01158 motor[TIRE_BL].pwm = 20; 01159 motor[TIRE_BL].dir = FOR; 01160 mtc2=true; 01161 break; 01162 case 2: 01163 motor[TIRE_FR].pwm = 30; 01164 motor[TIRE_FR].dir = BACK; 01165 motor[TIRE_FL].pwm = 0; 01166 motor[TIRE_FL].dir = FREE; 01167 motor[TIRE_BR].pwm = 0; 01168 motor[TIRE_BR].dir = FREE; 01169 motor[TIRE_BL].pwm = 30; 01170 motor[TIRE_BL].dir = FOR; 01171 mtc2=true; 01172 break; 01173 } 01174 if(g[2]!=98&&mtc2==true) { 01175 switch(g[2]) { 01176 case 0: 01177 mtc2=false; 01178 break; 01179 case 255: 01180 motor[TIRE_FL].pwm += 5; 01181 mtc2=false; 01182 break; 01183 case 253: 01184 motor[TIRE_FL].pwm += 10; 01185 mtc2=false; 01186 break; 01187 case 254: 01188 motor[TIRE_FL].pwm += 20; 01189 motor[TIRE_BL].pwm = 0; 01190 mtc2=false; 01191 break; 01192 case 1: 01193 motor[TIRE_BL].pwm += 5; 01194 mtc2=false; 01195 break; 01196 case 3: 01197 motor[TIRE_BL].pwm += 10; 01198 mtc2=false; 01199 break; 01200 case 2: 01201 motor[TIRE_BL].pwm += 20; 01202 motor[TIRE_FL].pwm = 0; 01203 mtc2=false; 01204 break; 01205 } 01206 } 01207 } else if(mode==41) { 01208 motor[TIRE_FR].pwm = 0; 01209 motor[TIRE_FR].dir = FREE; 01210 motor[TIRE_FL].pwm = 15; 01211 motor[TIRE_FL].dir = BACK; 01212 motor[TIRE_BR].pwm = 15; 01213 motor[TIRE_BR].dir = FOR; 01214 motor[TIRE_BL].pwm = 0; 01215 motor[TIRE_BL].dir = FREE; 01216 if(mode1g0>5000&&g[1]==0) { 01217 mode1g0=0; 01218 mode=42; 01219 } 01220 } else if(mode==42) { 01221 motor[TIRE_FR].pwm = 15; 01222 motor[TIRE_FR].dir = FOR; 01223 motor[TIRE_FL].pwm = 15; 01224 motor[TIRE_FL].dir = FOR; 01225 motor[TIRE_BR].pwm = 15; 01226 motor[TIRE_BR].dir = BACK; 01227 motor[TIRE_BL].pwm = 15; 01228 motor[TIRE_BL].dir = BACK; 01229 if(mode1g0>5000&&g[1]==0) { 01230 mode1g0=0; 01231 mode=50; 01232 } 01233 } else if(mode==50) { 01234 switch(g[1]) { 01235 case 98: 01236 motor[TIRE_FR].pwm = 30; 01237 motor[TIRE_FR].dir = FOR; 01238 motor[TIRE_FL].pwm = 30; 01239 motor[TIRE_FL].dir = BACK; 01240 motor[TIRE_BR].pwm = 30; 01241 motor[TIRE_BR].dir = FOR; 01242 motor[TIRE_BL].pwm = 30; 01243 motor[TIRE_BL].dir = BACK; 01244 if(mode1g0>100000) { 01245 mode1++; 01246 mode1g0=0; 01247 } 01248 mtc=true; 01249 if(mode1==1) { 01250 mode=51; 01251 mode1g0=0; 01252 } 01253 break; 01254 case 0: 01255 motor[TIRE_FR].pwm = 60; 01256 motor[TIRE_FR].dir = FOR; 01257 motor[TIRE_FL].pwm = 60; 01258 motor[TIRE_FL].dir = BACK; 01259 motor[TIRE_BR].pwm = 60; 01260 motor[TIRE_BR].dir = FOR; 01261 motor[TIRE_BL].pwm = 60; 01262 motor[TIRE_BL].dir = BACK; 01263 mtc=true; 01264 break; 01265 case 255: 01266 motor[TIRE_FR].pwm = 60; 01267 motor[TIRE_FR].dir = FOR; 01268 motor[TIRE_FL].pwm = 40; 01269 motor[TIRE_FL].dir = BACK; 01270 motor[TIRE_BR].pwm = 40; 01271 motor[TIRE_BR].dir = FOR; 01272 motor[TIRE_BL].pwm = 60; 01273 motor[TIRE_BL].dir = BACK; 01274 mtc=true; 01275 break; 01276 case 253: 01277 motor[TIRE_FR].pwm = 20; 01278 motor[TIRE_FR].dir = FOR; 01279 motor[TIRE_FL].pwm = 0; 01280 motor[TIRE_FL].dir = FREE; 01281 motor[TIRE_BR].pwm = 0; 01282 motor[TIRE_BR].dir = FREE; 01283 motor[TIRE_BL].pwm = 20; 01284 motor[TIRE_BL].dir = BACK; 01285 mtc=true; 01286 break; 01287 case 254: 01288 motor[TIRE_FR].pwm = 30; 01289 motor[TIRE_FR].dir = FOR; 01290 motor[TIRE_FL].pwm = 0; 01291 motor[TIRE_FL].dir = FREE; 01292 motor[TIRE_BR].pwm = 0; 01293 motor[TIRE_BR].dir = FREE; 01294 motor[TIRE_BL].pwm = 30; 01295 motor[TIRE_BL].dir = BACK; 01296 mtc=true; 01297 break; 01298 case 1: 01299 motor[TIRE_FR].pwm = 40; 01300 motor[TIRE_FR].dir = FOR; 01301 motor[TIRE_FL].pwm = 60; 01302 motor[TIRE_FL].dir = BACK; 01303 motor[TIRE_BR].pwm = 60; 01304 motor[TIRE_BR].dir = FOR; 01305 motor[TIRE_BL].pwm = 40; 01306 motor[TIRE_BL].dir = BACK; 01307 mtc=true; 01308 break; 01309 case 3: 01310 motor[TIRE_FR].pwm = 0; 01311 motor[TIRE_FR].dir = FREE; 01312 motor[TIRE_FL].pwm = 20; 01313 motor[TIRE_FL].dir = BACK; 01314 motor[TIRE_BR].pwm = 20; 01315 motor[TIRE_BR].dir = FOR; 01316 motor[TIRE_BL].pwm = 0; 01317 motor[TIRE_BL].dir = FREE; 01318 mtc=true; 01319 break; 01320 case 2: 01321 motor[TIRE_FR].pwm = 0; 01322 motor[TIRE_FR].dir = FREE; 01323 motor[TIRE_FL].pwm = 30; 01324 motor[TIRE_FL].dir = BACK; 01325 motor[TIRE_BR].pwm = 30; 01326 motor[TIRE_BR].dir = FOR; 01327 motor[TIRE_BL].pwm = 0; 01328 motor[TIRE_BL].dir = FREE; 01329 mtc=true; 01330 break; 01331 } 01332 if(g[1]!=98&&mtc==true) { 01333 switch(g[0]) { 01334 case 0: 01335 mtc=false; 01336 break; 01337 case 255: 01338 motor[TIRE_FR].pwm += 5; 01339 mtc=false; 01340 break; 01341 case 253: 01342 motor[TIRE_FR].pwm += 10; 01343 mtc=false; 01344 break; 01345 case 254: 01346 motor[TIRE_FR].pwm += 20; 01347 motor[TIRE_FL].pwm = 0; 01348 mtc=false; 01349 break; 01350 case 1: 01351 motor[TIRE_FL].pwm += 5; 01352 mtc=false; 01353 break; 01354 case 3: 01355 motor[TIRE_FL].pwm += 10; 01356 mtc=false; 01357 break; 01358 case 2: 01359 motor[TIRE_FL].pwm += 20; 01360 motor[TIRE_FR].pwm = 0; 01361 mtc=false; 01362 break; 01363 } 01364 } 01365 } else if(mode==51) { 01366 motor[TIRE_FR].pwm = 0; 01367 motor[TIRE_FR].dir = FREE; 01368 motor[TIRE_FL].pwm = 15; 01369 motor[TIRE_FL].dir = BACK; 01370 motor[TIRE_BR].pwm = 15; 01371 motor[TIRE_BR].dir = FOR; 01372 motor[TIRE_BL].pwm = 0; 01373 motor[TIRE_BL].dir = FREE; 01374 mode1g0++; 01375 if(mode1g0>5000&&g[3]==0) { 01376 mode1g0=0; 01377 mode=52; 01378 } 01379 } else if(mode==52) { 01380 motor[TIRE_FR].pwm = 15; 01381 motor[TIRE_FR].dir = BACK; 01382 motor[TIRE_FL].pwm = 15; 01383 motor[TIRE_FL].dir = FOR; 01384 motor[TIRE_BR].pwm = 15; 01385 motor[TIRE_BR].dir = BACK; 01386 motor[TIRE_BL].pwm = 15; 01387 motor[TIRE_BL].dir = FOR; 01388 mode1g0++; 01389 if(mode1g0>5000&&g[3]==0) { 01390 mode1g0=0; 01391 mode1=0; 01392 mode=60; 01393 } 01394 } else if(mode==60) { 01395 switch(g[3]) { 01396 case 98: 01397 motor[TIRE_FR].pwm = 30; 01398 motor[TIRE_FR].dir = BACK; 01399 motor[TIRE_FL].pwm = 30; 01400 motor[TIRE_FL].dir = BACK; 01401 motor[TIRE_BR].pwm = 30; 01402 motor[TIRE_BR].dir = FOR; 01403 motor[TIRE_BL].pwm = 30; 01404 motor[TIRE_BL].dir = FOR; 01405 if(mode1g0>100000) { 01406 mode1++; 01407 mode1g0=0; 01408 } 01409 mtc2=true; 01410 if(mode1==1) { 01411 mode=61; 01412 mode1g0=0; 01413 mode1=0; 01414 } 01415 break; 01416 case 0: 01417 motor[TIRE_FR].pwm = 60; 01418 motor[TIRE_FR].dir = BACK; 01419 motor[TIRE_FL].pwm = 60; 01420 motor[TIRE_FL].dir = BACK; 01421 motor[TIRE_BR].pwm = 60; 01422 motor[TIRE_BR].dir = FOR; 01423 motor[TIRE_BL].pwm = 60; 01424 motor[TIRE_BL].dir = FOR; 01425 mtc2=true; 01426 break; 01427 case 255: 01428 motor[TIRE_FR].pwm = 40; 01429 motor[TIRE_FR].dir = BACK; 01430 motor[TIRE_FL].pwm = 60; 01431 motor[TIRE_FL].dir = BACK; 01432 motor[TIRE_BR].pwm = 60; 01433 motor[TIRE_BR].dir = FOR; 01434 motor[TIRE_BL].pwm = 40; 01435 motor[TIRE_BL].dir = FOR; 01436 mtc2=true; 01437 break; 01438 case 253: 01439 motor[TIRE_FR].pwm = 0; 01440 motor[TIRE_FR].dir = FREE; 01441 motor[TIRE_FL].pwm = 20; 01442 motor[TIRE_FL].dir = BACK; 01443 motor[TIRE_BR].pwm = 20; 01444 motor[TIRE_BR].dir = FOR; 01445 motor[TIRE_BL].pwm = 0; 01446 motor[TIRE_BL].dir = FREE; 01447 mtc2=true; 01448 break; 01449 case 254: 01450 motor[TIRE_FR].pwm = 0; 01451 motor[TIRE_FR].dir = FREE; 01452 motor[TIRE_FL].pwm = 30; 01453 motor[TIRE_FL].dir = BACK; 01454 motor[TIRE_BR].pwm = 30; 01455 motor[TIRE_BR].dir = FOR; 01456 motor[TIRE_BL].pwm = 0; 01457 motor[TIRE_BL].dir = FREE; 01458 mtc2=true; 01459 break; 01460 case 1: 01461 motor[TIRE_FR].pwm = 60; 01462 motor[TIRE_FR].dir = BACK; 01463 motor[TIRE_FL].pwm = 40; 01464 motor[TIRE_FL].dir = BACK; 01465 motor[TIRE_BR].pwm = 40; 01466 motor[TIRE_BR].dir = FOR; 01467 motor[TIRE_BL].pwm = 60; 01468 motor[TIRE_BL].dir = FOR; 01469 mtc2=true; 01470 break; 01471 case 3: 01472 motor[TIRE_FR].pwm = 20; 01473 motor[TIRE_FR].dir = BACK; 01474 motor[TIRE_FL].pwm = 0; 01475 motor[TIRE_FL].dir = FREE; 01476 motor[TIRE_BR].pwm = 0; 01477 motor[TIRE_BR].dir = FREE; 01478 motor[TIRE_BL].pwm = 20; 01479 motor[TIRE_BL].dir = FOR; 01480 mtc2=true; 01481 break; 01482 case 2: 01483 motor[TIRE_FR].pwm = 30; 01484 motor[TIRE_FR].dir = BACK; 01485 motor[TIRE_FL].pwm = 0; 01486 motor[TIRE_FL].dir = FREE; 01487 motor[TIRE_BR].pwm = 0; 01488 motor[TIRE_BR].dir = FREE; 01489 motor[TIRE_BL].pwm = 30; 01490 motor[TIRE_BL].dir = FOR; 01491 mtc2=true; 01492 break; 01493 } 01494 if(g[2]!=98&&mtc2==true) { 01495 switch(g[2]) { 01496 case 0: 01497 mtc2=false; 01498 break; 01499 case 255: 01500 motor[TIRE_FL].pwm += 5; 01501 mtc2=false; 01502 break; 01503 case 253: 01504 motor[TIRE_FL].pwm += 10; 01505 mtc2=false; 01506 break; 01507 case 254: 01508 motor[TIRE_FL].pwm += 20; 01509 motor[TIRE_BL].pwm = 0; 01510 mtc2=false; 01511 break; 01512 case 1: 01513 motor[TIRE_BL].pwm += 5; 01514 mtc2=false; 01515 break; 01516 case 3: 01517 motor[TIRE_BL].pwm += 10; 01518 mtc2=false; 01519 break; 01520 case 2: 01521 motor[TIRE_BL].pwm += 20; 01522 motor[TIRE_FL].pwm = 0; 01523 mtc2=false; 01524 break; 01525 } 01526 } 01527 } else if(mode==61) { 01528 motor[TIRE_FR].pwm = 0; 01529 motor[TIRE_FR].dir = FREE; 01530 motor[TIRE_FL].pwm = 15; 01531 motor[TIRE_FL].dir = BACK; 01532 motor[TIRE_BR].pwm = 15; 01533 motor[TIRE_BR].dir = FOR; 01534 motor[TIRE_BL].pwm = 0; 01535 motor[TIRE_BL].dir = FREE; 01536 if(mode1g0>5000&&g[1]==0) { 01537 mode1g0=0; 01538 mode=62; 01539 } 01540 } else if(mode==62) { 01541 motor[TIRE_FR].pwm = 15; 01542 motor[TIRE_FR].dir = FOR; 01543 motor[TIRE_FL].pwm = 15; 01544 motor[TIRE_FL].dir = FOR; 01545 motor[TIRE_BR].pwm = 15; 01546 motor[TIRE_BR].dir = BACK; 01547 motor[TIRE_BL].pwm = 15; 01548 motor[TIRE_BL].dir = BACK; 01549 if(mode1g0>5000&&g[1]==0) { 01550 mode1g0=0; 01551 mode=70; 01552 } 01553 } else if(mode==70) { 01554 switch(g[1]) { 01555 case 98: 01556 motor[TIRE_FR].pwm = 30; 01557 motor[TIRE_FR].dir = FOR; 01558 motor[TIRE_FL].pwm = 30; 01559 motor[TIRE_FL].dir = BACK; 01560 motor[TIRE_BR].pwm = 30; 01561 motor[TIRE_BR].dir = FOR; 01562 motor[TIRE_BL].pwm = 30; 01563 motor[TIRE_BL].dir = BACK; 01564 if(mode1g0>100000) { 01565 mode1++; 01566 mode1g0=0; 01567 } 01568 mtc=true; 01569 if(mode1==1) { 01570 mode=71; 01571 mode1g0=0; 01572 } 01573 break; 01574 case 0: 01575 motor[TIRE_FR].pwm = 60; 01576 motor[TIRE_FR].dir = FOR; 01577 motor[TIRE_FL].pwm = 60; 01578 motor[TIRE_FL].dir = BACK; 01579 motor[TIRE_BR].pwm = 60; 01580 motor[TIRE_BR].dir = FOR; 01581 motor[TIRE_BL].pwm = 60; 01582 motor[TIRE_BL].dir = BACK; 01583 mtc=true; 01584 break; 01585 case 255: 01586 motor[TIRE_FR].pwm = 60; 01587 motor[TIRE_FR].dir = FOR; 01588 motor[TIRE_FL].pwm = 40; 01589 motor[TIRE_FL].dir = BACK; 01590 motor[TIRE_BR].pwm = 40; 01591 motor[TIRE_BR].dir = FOR; 01592 motor[TIRE_BL].pwm = 60; 01593 motor[TIRE_BL].dir = BACK; 01594 mtc=true; 01595 break; 01596 case 253: 01597 motor[TIRE_FR].pwm = 20; 01598 motor[TIRE_FR].dir = FOR; 01599 motor[TIRE_FL].pwm = 0; 01600 motor[TIRE_FL].dir = FREE; 01601 motor[TIRE_BR].pwm = 0; 01602 motor[TIRE_BR].dir = FREE; 01603 motor[TIRE_BL].pwm = 20; 01604 motor[TIRE_BL].dir = BACK; 01605 mtc=true; 01606 break; 01607 case 254: 01608 motor[TIRE_FR].pwm = 30; 01609 motor[TIRE_FR].dir = FOR; 01610 motor[TIRE_FL].pwm = 0; 01611 motor[TIRE_FL].dir = FREE; 01612 motor[TIRE_BR].pwm = 0; 01613 motor[TIRE_BR].dir = FREE; 01614 motor[TIRE_BL].pwm = 30; 01615 motor[TIRE_BL].dir = BACK; 01616 mtc=true; 01617 break; 01618 case 1: 01619 motor[TIRE_FR].pwm = 40; 01620 motor[TIRE_FR].dir = FOR; 01621 motor[TIRE_FL].pwm = 60; 01622 motor[TIRE_FL].dir = BACK; 01623 motor[TIRE_BR].pwm = 60; 01624 motor[TIRE_BR].dir = FOR; 01625 motor[TIRE_BL].pwm = 40; 01626 motor[TIRE_BL].dir = BACK; 01627 mtc=true; 01628 break; 01629 case 3: 01630 motor[TIRE_FR].pwm = 0; 01631 motor[TIRE_FR].dir = FREE; 01632 motor[TIRE_FL].pwm = 20; 01633 motor[TIRE_FL].dir = BACK; 01634 motor[TIRE_BR].pwm = 20; 01635 motor[TIRE_BR].dir = FOR; 01636 motor[TIRE_BL].pwm = 0; 01637 motor[TIRE_BL].dir = FREE; 01638 mtc=true; 01639 break; 01640 case 2: 01641 motor[TIRE_FR].pwm = 0; 01642 motor[TIRE_FR].dir = FREE; 01643 motor[TIRE_FL].pwm = 30; 01644 motor[TIRE_FL].dir = BACK; 01645 motor[TIRE_BR].pwm = 30; 01646 motor[TIRE_BR].dir = FOR; 01647 motor[TIRE_BL].pwm = 0; 01648 motor[TIRE_BL].dir = FREE; 01649 mtc=true; 01650 break; 01651 } 01652 if(g[1]!=98&&mtc==true) { 01653 switch(g[0]) { 01654 case 0: 01655 mtc=false; 01656 break; 01657 case 255: 01658 motor[TIRE_FR].pwm += 5; 01659 mtc=false; 01660 break; 01661 case 253: 01662 motor[TIRE_FR].pwm += 10; 01663 mtc=false; 01664 break; 01665 case 254: 01666 motor[TIRE_FR].pwm += 20; 01667 motor[TIRE_FL].pwm = 0; 01668 mtc=false; 01669 break; 01670 case 1: 01671 motor[TIRE_FL].pwm += 5; 01672 mtc=false; 01673 break; 01674 case 3: 01675 motor[TIRE_FL].pwm += 10; 01676 mtc=false; 01677 break; 01678 case 2: 01679 motor[TIRE_FL].pwm += 20; 01680 motor[TIRE_FR].pwm = 0; 01681 mtc=false; 01682 break; 01683 } 01684 } 01685 } else if(mode==71) { 01686 motor[TIRE_FR].pwm = 15; 01687 motor[TIRE_FR].dir = FOR; 01688 motor[TIRE_FL].pwm = 15; 01689 motor[TIRE_FL].dir = BACK; 01690 motor[TIRE_BR].pwm = 15; 01691 motor[TIRE_BR].dir = FOR; 01692 motor[TIRE_BL].pwm = 15; 01693 motor[TIRE_BL].dir = BACK; 01694 } else { 01695 motor[TIRE_FR].pwm = 0; 01696 motor[TIRE_FR].dir = BRAKE; 01697 motor[TIRE_FL].pwm = 0; 01698 motor[TIRE_FL].dir = BRAKE; 01699 motor[TIRE_BR].pwm = 0; 01700 motor[TIRE_BR].dir = BRAKE; 01701 motor[TIRE_BL].pwm = 0; 01702 motor[TIRE_BL].dir = BRAKE; 01703 } 01704 01705 } 01706 01707 #endif 01708 01709 #if USE_PROCESS_NUM>3 01710 static void Process3() 01711 { 01712 01713 } 01714 #endif 01715 01716 #if USE_PROCESS_NUM>4 01717 static void Process4() 01718 { 01719 01720 } 01721 #endif 01722 01723 #if USE_PROCESS_NUM>5 01724 static void Process5() 01725 { 01726 01727 } 01728 #endif 01729 01730 #if USE_PROCESS_NUM>6 01731 static void Process6() 01732 { 01733 01734 } 01735 #endif 01736 01737 #if USE_PROCESS_NUM>7 01738 static void Process7() 01739 { 01740 01741 } 01742 #endif 01743 01744 #if USE_PROCESS_NUM>8 01745 static void Process8() 01746 { 01747 01748 } 01749 #endif 01750 01751 #if USE_PROCESS_NUM>9 01752 static void Process9() 01753 { 01754 01755 } 01756 #endif 01757 #endif 01758 #pragma endregion PROCESS 01759 01760 static void AllActuatorReset() 01761 { 01762 01763 #ifdef USE_SOLENOID 01764 solenoid.all = ALL_SOLENOID_OFF; 01765 #endif 01766 01767 #ifdef USE_MOTOR 01768 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 01769 motor[i].dir = FREE; 01770 motor[i].pwm = 0; 01771 } 01772 #endif 01773 } 01774 01775 /* 01776 void BuzzerTimer_func() 01777 { 01778 buzzer = !buzzer; 01779 //LED_DEBUG0 = !LED_DEBUG0; 01780 } 01781 */ 01782 void TapeLedEms_func() 01783 { 01784 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 01785 } 01786 01787 #pragma region USER-DEFINED-FUNCTIONS 01788 01789 01790 #pragma endregion
Generated on Thu Jul 14 2022 16:46:47 by
1.7.2