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