aa
Dependencies: mbed TrapezoidControl QEI
Process.cpp
00001 00002 #include "mbed.h" 00003 #include "Process.h" 00004 #include "QEI.h" 00005 00006 #include "../../CommonLibraries/PID/PID.h" 00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" 00008 #include "../../Communication/RS485/LineHub/LineHub.h" 00009 #include "../../Communication/Controller/Controller.h" 00010 #include "../../Input/ExternalInt/ExternalInt.h" 00011 #include "../../Input/Switch/Switch.h" 00012 #include "../../Input/Potentiometer/Potentiometer.h" 00013 #include "../../Input/Encoder/Encoder.h" 00014 #include "../../LED/LED.h" 00015 #include "../../Safty/Safty.h" 00016 #include "../Using.h" 00017 00018 using namespace SWITCH; 00019 using namespace PID_SPACE; 00020 using namespace ENCODER; 00021 using namespace LINEHUB; 00022 00023 static CONTROLLER::ControllerData *controller; 00024 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00025 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; 00026 00027 static bool lock; 00028 static bool processChangeComp; 00029 static int current; 00030 00031 static void AllActuatorReset(); 00032 00033 #ifdef USE_SUBPROCESS 00034 static void (*Process[USE_PROCESS_NUM])(void); 00035 #endif 00036 00037 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE 00038 00039 /*Replace here with the definition code of your variables.*/ 00040 00041 Serial pc(USBTX, USBRX); 00042 00043 //**************Encoder*************** 00044 const int PerRev = 256; 00045 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING); 00046 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING); 00047 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING); 00048 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING); 00049 //**************Encoder*************** 00050 00051 //**************Buzzer**************** 00052 //DigitalOut buzzer(BUZZER_PIN); 00053 void BuzzerTimer_func(); 00054 Ticker BuzzerTimer; 00055 bool EMGflag = false; 00056 //PWMOut buzzer(BUZZER_PIN); 00057 //**************Buzzer**************** 00058 00059 //************TapeLed***************** 00060 void TapeLedEms_func(); 00061 TapeLedData tapeLED; 00062 TapeLedData sendLedData; 00063 TapeLED_Mode ledMode = Normal; 00064 Ticker tapeLedTimer; 00065 //************TapaLed***************** 00066 00067 float tireProRPM[4]; 00068 float tireTarRPM[4]; 00069 float tirepwm[4]; 00070 00071 const int omni[15][15] = { 00072 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, 00073 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, 00074 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, 00075 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, 00076 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, 00077 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, 00078 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, 00079 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, 00080 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, 00081 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, 00082 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, 00083 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, 00084 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, 00085 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, 00086 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } 00087 }; 00088 00089 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 }; 00090 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200} 00091 00092 uint8_t SetStatus(int); 00093 uint8_t SetStatus(int pwmVal) 00094 { 00095 if (pwmVal < 0) return BACK; 00096 else if (pwmVal > 0) return FOR; 00097 else if (pwmVal == 0) return BRAKE; 00098 else return BRAKE; 00099 } 00100 uint8_t Setpwm(int); 00101 uint8_t Setpwm(int pwmVal) 00102 { 00103 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; 00104 else return abs(pwmVal); 00105 } 00106 00107 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00108 00109 #ifdef USE_SUBPROCESS 00110 #if USE_PROCESS_NUM>0 00111 static void Process0(void); 00112 #endif 00113 #if USE_PROCESS_NUM>1 00114 static void Process1(void); 00115 #endif 00116 #if USE_PROCESS_NUM>2 00117 static void Process2(void); 00118 #endif 00119 #if USE_PROCESS_NUM>3 00120 static void Process3(void); 00121 #endif 00122 #if USE_PROCESS_NUM>4 00123 static void Process4(void); 00124 #endif 00125 #if USE_PROCESS_NUM>5 00126 static void Process5(void); 00127 #endif 00128 #if USE_PROCESS_NUM>6 00129 static void Process6(void); 00130 #endif 00131 #if USE_PROCESS_NUM>7 00132 static void Process7(void); 00133 #endif 00134 #if USE_PROCESS_NUM>8 00135 static void Process8(void); 00136 #endif 00137 #if USE_PROCESS_NUM>9 00138 static void Process9(void); 00139 #endif 00140 #endif 00141 00142 void SystemProcessInitialize() 00143 { 00144 #pragma region USER-DEFINED_VARIABLE_INIT 00145 /*Replace here with the initialization code of your variables.*/ 00146 #pragma endregion USER-DEFINED_VARIABLE_INIT 00147 00148 lock = true; 00149 processChangeComp = true; 00150 current = DEFAULT_PROCESS; 00151 00152 #ifdef USE_SUBPROCESS 00153 #if USE_PROCESS_NUM>0 00154 Process[0] = Process0; 00155 #endif 00156 #if USE_PROCESS_NUM>1 00157 Process[1] = Process1; 00158 #endif 00159 #if USE_PROCESS_NUM>2 00160 Process[2] = Process2; 00161 #endif 00162 #if USE_PROCESS_NUM>3 00163 Process[3] = Process3; 00164 #endif 00165 #if USE_PROCESS_NUM>4 00166 Process[4] = Process4; 00167 #endif 00168 #if USE_PROCESS_NUM>5 00169 Process[5] = Process5; 00170 #endif 00171 #if USE_PROCESS_NUM>6 00172 Process[6] = Process6; 00173 #endif 00174 #if USE_PROCESS_NUM>7 00175 Process[7] = Process7; 00176 #endif 00177 #if USE_PROCESS_NUM>8 00178 Process[8] = Process8; 00179 #endif 00180 #if USE_PROCESS_NUM>9 00181 Process[9] = Process9; 00182 #endif 00183 #endif 00184 } 00185 00186 static void SystemProcessUpdate() 00187 { 00188 #ifdef USE_SUBPROCESS 00189 if(controller->Button.HOME) lock = false; 00190 00191 if(controller->Button.START && processChangeComp) { 00192 current++; 00193 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00194 processChangeComp = false; 00195 } else if(controller->Button.SELECT && processChangeComp) { 00196 current--; 00197 if (current < 0) current = 0; 00198 processChangeComp = false; 00199 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00200 #endif 00201 00202 #ifdef USE_MOTOR 00203 ACTUATORHUB::MOTOR::Motor::Update(motor); 00204 #endif 00205 00206 #ifdef USE_SOLENOID 00207 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00208 #endif 00209 00210 #ifdef USE_RS485 00211 ACTUATORHUB::ActuatorHub::Update(); 00212 #endif 00213 00214 } 00215 00216 int g[8]; 00217 00218 void SystemProcess() 00219 { 00220 SystemProcessInitialize(); 00221 00222 while(1) { 00223 for(int i = 0; i < 8; i++) { 00224 g[i] = LineHub::GetPara(i); 00225 } 00226 00227 #ifdef USE_MU 00228 controller = CONTROLLER::Controller::GetData(); 00229 #endif 00230 00231 #ifdef USE_ERRORCHECK 00232 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { 00233 CONTROLLER::Controller::DataReset(); 00234 AllActuatorReset(); 00235 lock = true; 00236 } else 00237 #endif 00238 { 00239 00240 #ifdef USE_SUBPROCESS 00241 if(!lock) { 00242 Process[current](); 00243 } else 00244 #endif 00245 { 00246 //ロック時の処理 00247 } 00248 } 00249 /* 00250 //Emergency! 00251 if(!EMG_0 && !EMG_1 && !EMGflag) { 00252 buzzer = 0; 00253 BuzzerTimer.attach(BuzzerTimer_func, 1); 00254 EMGflag = true; 00255 LED_DEBUG0 = 1; 00256 } 00257 if(EMG_0 && EMG_1 && EMGflag) { 00258 buzzer = 1; 00259 BuzzerTimer.detach(); 00260 EMGflag = false; 00261 } 00262 */ 00263 SystemProcessUpdate(); 00264 } 00265 } 00266 00267 00268 int Twsh; 00269 00270 #pragma region PROCESS 00271 #ifdef USE_SUBPROCESS 00272 #if USE_PROCESS_NUM>0 00273 00274 static void Process0() 00275 { 00276 if(StertSW) { 00277 if(ZoneSW) { 00278 current = 1; 00279 Twsh=2; 00280 } else { 00281 current = 1; 00282 Twsh=3; 00283 } 00284 } 00285 } 00286 #endif 00287 00288 #if USE_PROCESS_NUM>1 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 =1; 00300 int cross=0;//十字数える用 00301 int count=100000;//wait代替え 00302 00303 00304 static void Process1() 00305 { 00306 count=100000; 00307 mode=1; 00308 cross=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 count++; 00420 if(mode==1) {//スタートゾーンから白線検知 00421 motor[TIRE_FR].pwm = 30; 00422 motor[TIRE_FR].dir = BACK; 00423 motor[TIRE_FL].pwm = 30; 00424 motor[TIRE_FL].dir = FOR; 00425 motor[TIRE_BR].pwm = 30; 00426 motor[TIRE_BR].dir = BACK; 00427 motor[TIRE_BL].pwm = 30; 00428 motor[TIRE_BL].dir = FOR; 00429 if(g[2]==0) { 00430 count=100000; 00431 cross=0; 00432 mode=2; 00433 } 00434 } else if(mode==2) {//横移動 00435 if(g[2]==0) { 00436 count=100000; 00437 cross=0; 00438 mode=3; 00439 mtc2=true; 00440 } else { 00441 motor[TIRE_FR].pwm = 30; 00442 motor[TIRE_FR].dir = FOR; 00443 motor[TIRE_FL].pwm = 30; 00444 motor[TIRE_FL].dir = FOR; 00445 motor[TIRE_BR].pwm = 30; 00446 motor[TIRE_BR].dir = BACK; 00447 motor[TIRE_BL].pwm = 30; 00448 motor[TIRE_BL].dir = BACK; 00449 } 00450 } else if(mode==3) { //横ライントレースから縦ライントレースへ 00451 motor[TIRE_FR].pwm = 0; 00452 motor[TIRE_FR].dir = FREE; 00453 motor[TIRE_FL].pwm = 20; 00454 motor[TIRE_FL].dir = FOR; 00455 motor[TIRE_BR].pwm = 20; 00456 motor[TIRE_BR].dir = BACK; 00457 motor[TIRE_BL].pwm = 0; 00458 motor[TIRE_BL].dir = FREE; 00459 if(count>100000&&g[0]==0) { 00460 count=0; 00461 mode=10; 00462 } 00463 } else if(mode==4) { //横ライントレースから縦ライントレースへ 00464 motor[TIRE_FR].pwm = 15; 00465 motor[TIRE_FR].dir = BACK; 00466 motor[TIRE_FL].pwm = 15; 00467 motor[TIRE_FL].dir = BACK; 00468 motor[TIRE_BR].pwm = 15; 00469 motor[TIRE_BR].dir = FOR; 00470 motor[TIRE_BL].pwm = 15; 00471 motor[TIRE_BL].dir = FOR; 00472 if(count>30000&&g[0]==0) { 00473 count=0; 00474 cross=0; 00475 mode=10; 00476 } 00477 } else if(g[0]!=99&&mode==10) { //縦ライントレース 00478 switch(g[0]) { 00479 case 98: 00480 motor[TIRE_FR].pwm = 30; 00481 motor[TIRE_FR].dir = BACK; 00482 motor[TIRE_FL].pwm = 30; 00483 motor[TIRE_FL].dir = FOR; 00484 motor[TIRE_BR].pwm = 30; 00485 motor[TIRE_BR].dir = BACK; 00486 motor[TIRE_BL].pwm = 30; 00487 motor[TIRE_BL].dir = FOR; 00488 if(count>100000) { 00489 cross++; 00490 count=0; 00491 } 00492 mtc=true; 00493 if(cross==1) { 00494 mode=11; 00495 count=0; 00496 } 00497 break; 00498 case 0: 00499 motor[TIRE_FR].pwm = 60; 00500 motor[TIRE_FR].dir = BACK; 00501 motor[TIRE_FL].pwm = 60; 00502 motor[TIRE_FL].dir = FOR; 00503 motor[TIRE_BR].pwm = 60; 00504 motor[TIRE_BR].dir = BACK; 00505 motor[TIRE_BL].pwm = 60; 00506 motor[TIRE_BL].dir = FOR; 00507 mtc=true; 00508 break; 00509 case 255: 00510 motor[TIRE_FR].pwm = 60; 00511 motor[TIRE_FR].dir = BACK; 00512 motor[TIRE_FL].pwm = 40; 00513 motor[TIRE_FL].dir = FOR; 00514 motor[TIRE_BR].pwm = 40; 00515 motor[TIRE_BR].dir = BACK; 00516 motor[TIRE_BL].pwm = 60; 00517 motor[TIRE_BL].dir = FOR; 00518 mtc=true; 00519 break; 00520 case 253: 00521 motor[TIRE_FR].pwm = 20; 00522 motor[TIRE_FR].dir = BACK; 00523 motor[TIRE_FL].pwm = 0; 00524 motor[TIRE_FL].dir = FREE; 00525 motor[TIRE_BR].pwm = 0; 00526 motor[TIRE_BR].dir = FREE; 00527 motor[TIRE_BL].pwm = 20; 00528 motor[TIRE_BL].dir = FOR; 00529 mtc=true; 00530 break; 00531 case 254: 00532 motor[TIRE_FR].pwm = 30; 00533 motor[TIRE_FR].dir = BACK; 00534 motor[TIRE_FL].pwm = 0; 00535 motor[TIRE_FL].dir = FREE; 00536 motor[TIRE_BR].pwm = 0; 00537 motor[TIRE_BR].dir = FREE; 00538 motor[TIRE_BL].pwm = 30; 00539 motor[TIRE_BL].dir = FOR; 00540 mtc=true; 00541 break; 00542 case 1: 00543 motor[TIRE_FR].pwm = 40; 00544 motor[TIRE_FR].dir = BACK; 00545 motor[TIRE_FL].pwm = 60; 00546 motor[TIRE_FL].dir = FOR; 00547 motor[TIRE_BR].pwm = 60; 00548 motor[TIRE_BR].dir = BACK; 00549 motor[TIRE_BL].pwm = 40; 00550 motor[TIRE_BL].dir = FOR; 00551 mtc=true; 00552 break; 00553 case 3: 00554 motor[TIRE_FR].pwm = 0; 00555 motor[TIRE_FR].dir = FREE; 00556 motor[TIRE_FL].pwm = 20; 00557 motor[TIRE_FL].dir = FOR; 00558 motor[TIRE_BR].pwm = 20; 00559 motor[TIRE_BR].dir = BACK; 00560 motor[TIRE_BL].pwm = 0; 00561 motor[TIRE_BL].dir = FREE; 00562 mtc=true; 00563 break; 00564 case 2: 00565 motor[TIRE_FR].pwm = 0; 00566 motor[TIRE_FR].dir = FREE; 00567 motor[TIRE_FL].pwm = 30; 00568 motor[TIRE_FL].dir = FOR; 00569 motor[TIRE_BR].pwm = 30; 00570 motor[TIRE_BR].dir = BACK; 00571 motor[TIRE_BL].pwm = 0; 00572 motor[TIRE_BL].dir = FREE; 00573 mtc=true; 00574 break; 00575 } 00576 if(g[0]!=98&&mtc==true) { 00577 switch(g[1]) { 00578 case 0: 00579 mtc=false; 00580 break; 00581 case 255: 00582 motor[TIRE_BR].pwm += 5; 00583 mtc=false; 00584 break; 00585 case 253: 00586 motor[TIRE_BR].pwm += 10; 00587 mtc=false; 00588 break; 00589 case 254: 00590 motor[TIRE_BR].pwm += 20; 00591 motor[TIRE_BL].pwm = 0; 00592 mtc=false; 00593 break; 00594 case 1: 00595 motor[TIRE_BL].pwm += 5; 00596 mtc=false; 00597 break; 00598 case 3: 00599 motor[TIRE_BL].pwm += 10; 00600 mtc=false; 00601 break; 00602 case 2: 00603 motor[TIRE_BL].pwm += 20; 00604 motor[TIRE_BR].pwm = 0; 00605 mtc=false; 00606 break; 00607 } 00608 } 00609 } else if(mode==11) { 00610 motor[TIRE_FR].pwm = 0; 00611 motor[TIRE_FR].dir = FREE; 00612 motor[TIRE_FL].pwm = 15; 00613 motor[TIRE_FL].dir = FOR; 00614 motor[TIRE_BR].pwm = 15; 00615 motor[TIRE_BR].dir = BACK; 00616 motor[TIRE_BL].pwm = 0; 00617 motor[TIRE_BL].dir = FREE; 00618 if(count>5000&&g[2]==0) { 00619 count=0; 00620 mode=12; 00621 } 00622 } else if(mode==12) { 00623 motor[TIRE_FR].pwm = 15; 00624 motor[TIRE_FR].dir = FOR; 00625 motor[TIRE_FL].pwm = 15; 00626 motor[TIRE_FL].dir = BACK; 00627 motor[TIRE_BR].pwm = 15; 00628 motor[TIRE_BR].dir = FOR; 00629 motor[TIRE_BL].pwm = 15; 00630 motor[TIRE_BL].dir = BACK; 00631 if(count>10000&&g[2]==0) { 00632 count=0; 00633 cross=0; 00634 mode=20; 00635 } 00636 } else if(mode==20) { 00637 switch(g[2]) { 00638 case 98: 00639 motor[TIRE_FR].pwm = 30; 00640 motor[TIRE_FR].dir = FOR; 00641 motor[TIRE_FL].pwm = 30; 00642 motor[TIRE_FL].dir = FOR; 00643 motor[TIRE_BR].pwm = 30; 00644 motor[TIRE_BR].dir = BACK; 00645 motor[TIRE_BL].pwm = 30; 00646 motor[TIRE_BL].dir = BACK; 00647 if(count>50000) { 00648 cross++; 00649 count=0; 00650 } 00651 mtc2=true; 00652 if(cross==3) { 00653 mode=21; 00654 count=0; 00655 cross=0; 00656 } 00657 break; 00658 case 0: 00659 motor[TIRE_FR].pwm = 60; 00660 motor[TIRE_FR].dir = FOR; 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 = 60; 00666 motor[TIRE_BL].dir = BACK; 00667 mtc2=true; 00668 break; 00669 case 255: 00670 motor[TIRE_FR].pwm = 40; 00671 motor[TIRE_FR].dir = FOR; 00672 motor[TIRE_FL].pwm = 60; 00673 motor[TIRE_FL].dir = FOR; 00674 motor[TIRE_BR].pwm = 60; 00675 motor[TIRE_BR].dir = BACK; 00676 motor[TIRE_BL].pwm = 40; 00677 motor[TIRE_BL].dir = BACK; 00678 mtc2=true; 00679 break; 00680 case 253: 00681 motor[TIRE_FR].pwm = 0; 00682 motor[TIRE_FR].dir = FREE; 00683 motor[TIRE_FL].pwm = 20; 00684 motor[TIRE_FL].dir = FOR; 00685 motor[TIRE_BR].pwm = 20; 00686 motor[TIRE_BR].dir = BACK; 00687 motor[TIRE_BL].pwm = 0; 00688 motor[TIRE_BL].dir = FREE; 00689 mtc2=true; 00690 break; 00691 case 254: 00692 motor[TIRE_FR].pwm = 0; 00693 motor[TIRE_FR].dir = FREE; 00694 motor[TIRE_FL].pwm = 30; 00695 motor[TIRE_FL].dir = FOR; 00696 motor[TIRE_BR].pwm = 30; 00697 motor[TIRE_BR].dir = BACK; 00698 motor[TIRE_BL].pwm = 0; 00699 motor[TIRE_BL].dir = FREE; 00700 mtc2=true; 00701 break; 00702 case 1: 00703 motor[TIRE_FR].pwm = 60; 00704 motor[TIRE_FR].dir = FOR; 00705 motor[TIRE_FL].pwm = 40; 00706 motor[TIRE_FL].dir = FOR; 00707 motor[TIRE_BR].pwm = 40; 00708 motor[TIRE_BR].dir = BACK; 00709 motor[TIRE_BL].pwm = 60; 00710 motor[TIRE_BL].dir = BACK; 00711 mtc2=true; 00712 break; 00713 case 3: 00714 motor[TIRE_FR].pwm = 20; 00715 motor[TIRE_FR].dir = FOR; 00716 motor[TIRE_FL].pwm = 0; 00717 motor[TIRE_FL].dir = FREE; 00718 motor[TIRE_BR].pwm = 0; 00719 motor[TIRE_BR].dir = FREE; 00720 motor[TIRE_BL].pwm = 20; 00721 motor[TIRE_BL].dir = BACK; 00722 mtc2=true; 00723 break; 00724 case 2: 00725 motor[TIRE_FR].pwm = 30; 00726 motor[TIRE_FR].dir = FOR; 00727 motor[TIRE_FL].pwm = 0; 00728 motor[TIRE_FL].dir = FREE; 00729 motor[TIRE_BR].pwm = 0; 00730 motor[TIRE_BR].dir = FREE; 00731 motor[TIRE_BL].pwm = 30; 00732 motor[TIRE_BL].dir = BACK; 00733 mtc2=true; 00734 break; 00735 } 00736 if(g[2]!=98&&mtc2==true) { 00737 switch(g[3]) { 00738 case 0: 00739 mtc2=false; 00740 break; 00741 case 255: 00742 motor[TIRE_BL].pwm += 5; 00743 mtc2=false; 00744 break; 00745 case 253: 00746 motor[TIRE_BL].pwm += 10; 00747 mtc2=false; 00748 break; 00749 case 254: 00750 motor[TIRE_BL].pwm += 20; 00751 motor[TIRE_FL].pwm = 0; 00752 mtc2=false; 00753 break; 00754 case 1: 00755 motor[TIRE_FL].pwm += 5; 00756 mtc2=false; 00757 break; 00758 case 3: 00759 motor[TIRE_FL].pwm += 10; 00760 mtc2=false; 00761 break; 00762 case 2: 00763 motor[TIRE_FL].pwm += 20; 00764 motor[TIRE_BL].pwm = 0; 00765 mtc2=false; 00766 break; 00767 } 00768 } 00769 } else if(mode==21) { 00770 motor[TIRE_FR].pwm = 0; 00771 motor[TIRE_FR].dir = FREE; 00772 motor[TIRE_FL].pwm = 15; 00773 motor[TIRE_FL].dir = FOR; 00774 motor[TIRE_BR].pwm = 15; 00775 motor[TIRE_BR].dir = BACK; 00776 motor[TIRE_BL].pwm = 0; 00777 motor[TIRE_BL].dir = FREE; 00778 if(count>5000&&g[0]==0) { 00779 count=0; 00780 mode=22; 00781 } 00782 } else if(mode==22) { 00783 motor[TIRE_FR].pwm = 15; 00784 motor[TIRE_FR].dir = BACK; 00785 motor[TIRE_FL].pwm = 15; 00786 motor[TIRE_FL].dir = BACK; 00787 motor[TIRE_BR].pwm = 15; 00788 motor[TIRE_BR].dir = FOR; 00789 motor[TIRE_BL].pwm = 15; 00790 motor[TIRE_BL].dir = FOR; 00791 if(count>10000&&g[0]==0) { 00792 count=0; 00793 cross=0; 00794 mode=30; 00795 } 00796 } else if(mode==30) { 00797 switch(g[0]) { 00798 case 98: 00799 motor[TIRE_FR].pwm = 30; 00800 motor[TIRE_FR].dir = BACK; 00801 motor[TIRE_FL].pwm = 30; 00802 motor[TIRE_FL].dir = FOR; 00803 motor[TIRE_BR].pwm = 30; 00804 motor[TIRE_BR].dir = BACK; 00805 motor[TIRE_BL].pwm = 30; 00806 motor[TIRE_BL].dir = FOR; 00807 if(count>100000) { 00808 cross++; 00809 count=0; 00810 } 00811 mtc=true; 00812 if(cross==1) { 00813 mode=31; 00814 count=0; 00815 } 00816 break; 00817 case 0: 00818 motor[TIRE_FR].pwm = 60; 00819 motor[TIRE_FR].dir = BACK; 00820 motor[TIRE_FL].pwm = 60; 00821 motor[TIRE_FL].dir = FOR; 00822 motor[TIRE_BR].pwm = 60; 00823 motor[TIRE_BR].dir = BACK; 00824 motor[TIRE_BL].pwm = 60; 00825 motor[TIRE_BL].dir = FOR; 00826 mtc=true; 00827 break; 00828 case 255: 00829 motor[TIRE_FR].pwm = 60; 00830 motor[TIRE_FR].dir = BACK; 00831 motor[TIRE_FL].pwm = 40; 00832 motor[TIRE_FL].dir = FOR; 00833 motor[TIRE_BR].pwm = 40; 00834 motor[TIRE_BR].dir = BACK; 00835 motor[TIRE_BL].pwm = 60; 00836 motor[TIRE_BL].dir = FOR; 00837 mtc=true; 00838 break; 00839 case 253: 00840 motor[TIRE_FR].pwm = 20; 00841 motor[TIRE_FR].dir = BACK; 00842 motor[TIRE_FL].pwm = 0; 00843 motor[TIRE_FL].dir = FREE; 00844 motor[TIRE_BR].pwm = 0; 00845 motor[TIRE_BR].dir = FREE; 00846 motor[TIRE_BL].pwm = 20; 00847 motor[TIRE_BL].dir = FOR; 00848 mtc=true; 00849 break; 00850 case 254: 00851 motor[TIRE_FR].pwm = 30; 00852 motor[TIRE_FR].dir = BACK; 00853 motor[TIRE_FL].pwm = 0; 00854 motor[TIRE_FL].dir = FREE; 00855 motor[TIRE_BR].pwm = 0; 00856 motor[TIRE_BR].dir = FREE; 00857 motor[TIRE_BL].pwm = 30; 00858 motor[TIRE_BL].dir = FOR; 00859 mtc=true; 00860 break; 00861 case 1: 00862 motor[TIRE_FR].pwm = 40; 00863 motor[TIRE_FR].dir = BACK; 00864 motor[TIRE_FL].pwm = 60; 00865 motor[TIRE_FL].dir = FOR; 00866 motor[TIRE_BR].pwm = 60; 00867 motor[TIRE_BR].dir = BACK; 00868 motor[TIRE_BL].pwm = 40; 00869 motor[TIRE_BL].dir = FOR; 00870 mtc=true; 00871 break; 00872 case 3: 00873 motor[TIRE_FR].pwm = 0; 00874 motor[TIRE_FR].dir = FREE; 00875 motor[TIRE_FL].pwm = 20; 00876 motor[TIRE_FL].dir = FOR; 00877 motor[TIRE_BR].pwm = 20; 00878 motor[TIRE_BR].dir = BACK; 00879 motor[TIRE_BL].pwm = 0; 00880 motor[TIRE_BL].dir = FREE; 00881 mtc=true; 00882 break; 00883 case 2: 00884 motor[TIRE_FR].pwm = 0; 00885 motor[TIRE_FR].dir = FREE; 00886 motor[TIRE_FL].pwm = 30; 00887 motor[TIRE_FL].dir = FOR; 00888 motor[TIRE_BR].pwm = 30; 00889 motor[TIRE_BR].dir = BACK; 00890 motor[TIRE_BL].pwm = 0; 00891 motor[TIRE_BL].dir = FREE; 00892 mtc=true; 00893 break; 00894 } 00895 if(g[0]!=98&&mtc==true) { 00896 switch(g[1]) { 00897 case 0: 00898 mtc=false; 00899 break; 00900 case 255: 00901 motor[TIRE_BR].pwm += 5; 00902 mtc=false; 00903 break; 00904 case 253: 00905 motor[TIRE_BR].pwm += 10; 00906 mtc=false; 00907 break; 00908 case 254: 00909 motor[TIRE_BR].pwm += 20; 00910 motor[TIRE_BL].pwm = 0; 00911 mtc=false; 00912 break; 00913 case 1: 00914 motor[TIRE_BL].pwm += 5; 00915 mtc=false; 00916 break; 00917 case 3: 00918 motor[TIRE_BL].pwm += 10; 00919 mtc=false; 00920 break; 00921 case 2: 00922 motor[TIRE_BL].pwm += 20; 00923 motor[TIRE_BR].pwm = 0; 00924 mtc=false; 00925 break; 00926 } 00927 } 00928 } else if(mode==31) { 00929 motor[TIRE_FR].pwm = 15; 00930 motor[TIRE_FR].dir = BACK; 00931 motor[TIRE_FL].pwm = 0; 00932 motor[TIRE_FL].dir = FREE; 00933 motor[TIRE_BR].pwm = 0; 00934 motor[TIRE_BR].dir = FREE; 00935 motor[TIRE_BL].pwm = 15; 00936 motor[TIRE_BL].dir = FOR; 00937 if(count>5000&&g[3]==0) { 00938 count=0; 00939 mode=32; 00940 } 00941 } else if(mode==32) { 00942 motor[TIRE_FR].pwm = 15; 00943 motor[TIRE_FR].dir = FOR; 00944 motor[TIRE_FL].pwm = 15; 00945 motor[TIRE_FL].dir = BACK; 00946 motor[TIRE_BR].pwm = 15; 00947 motor[TIRE_BR].dir = FOR; 00948 motor[TIRE_BL].pwm = 15; 00949 motor[TIRE_BL].dir = BACK; 00950 if(count>5000&&g[3]==0) { 00951 count=0; 00952 cross=0; 00953 mode=40; 00954 } 00955 } else if(mode==40) { 00956 switch(g[3]) { 00957 case 98: 00958 motor[TIRE_FR].pwm = 30; 00959 motor[TIRE_FR].dir = BACK; 00960 motor[TIRE_FL].pwm = 30; 00961 motor[TIRE_FL].dir = BACK; 00962 motor[TIRE_BR].pwm = 30; 00963 motor[TIRE_BR].dir = FOR; 00964 motor[TIRE_BL].pwm = 30; 00965 motor[TIRE_BL].dir = FOR; 00966 if(count>30000) { 00967 cross++; 00968 count=0; 00969 } 00970 mtc2=true; 00971 if(cross==1) { 00972 mode=41; 00973 count=0; 00974 cross=0; 00975 } 00976 break; 00977 case 0: 00978 motor[TIRE_FR].pwm = 60; 00979 motor[TIRE_FR].dir = BACK; 00980 motor[TIRE_FL].pwm = 60; 00981 motor[TIRE_FL].dir = BACK; 00982 motor[TIRE_BR].pwm = 60; 00983 motor[TIRE_BR].dir = FOR; 00984 motor[TIRE_BL].pwm = 60; 00985 motor[TIRE_BL].dir = FOR; 00986 mtc2=true; 00987 break; 00988 case 255: 00989 motor[TIRE_FR].pwm = 40; 00990 motor[TIRE_FR].dir = BACK; 00991 motor[TIRE_FL].pwm = 60; 00992 motor[TIRE_FL].dir = BACK; 00993 motor[TIRE_BR].pwm = 60; 00994 motor[TIRE_BR].dir = FOR; 00995 motor[TIRE_BL].pwm = 40; 00996 motor[TIRE_BL].dir = FOR; 00997 mtc2=true; 00998 break; 00999 case 253: 01000 motor[TIRE_FR].pwm = 0; 01001 motor[TIRE_FR].dir = FREE; 01002 motor[TIRE_FL].pwm = 20; 01003 motor[TIRE_FL].dir = BACK; 01004 motor[TIRE_BR].pwm = 20; 01005 motor[TIRE_BR].dir = FOR; 01006 motor[TIRE_BL].pwm = 0; 01007 motor[TIRE_BL].dir = FREE; 01008 mtc2=true; 01009 break; 01010 case 254: 01011 motor[TIRE_FR].pwm = 0; 01012 motor[TIRE_FR].dir = FREE; 01013 motor[TIRE_FL].pwm = 30; 01014 motor[TIRE_FL].dir = BACK; 01015 motor[TIRE_BR].pwm = 30; 01016 motor[TIRE_BR].dir = FOR; 01017 motor[TIRE_BL].pwm = 0; 01018 motor[TIRE_BL].dir = FREE; 01019 mtc2=true; 01020 break; 01021 case 1: 01022 motor[TIRE_FR].pwm = 60; 01023 motor[TIRE_FR].dir = BACK; 01024 motor[TIRE_FL].pwm = 40; 01025 motor[TIRE_FL].dir = BACK; 01026 motor[TIRE_BR].pwm = 40; 01027 motor[TIRE_BR].dir = FOR; 01028 motor[TIRE_BL].pwm = 60; 01029 motor[TIRE_BL].dir = FOR; 01030 mtc2=true; 01031 break; 01032 case 3: 01033 motor[TIRE_FR].pwm = 20; 01034 motor[TIRE_FR].dir = BACK; 01035 motor[TIRE_FL].pwm = 0; 01036 motor[TIRE_FL].dir = FREE; 01037 motor[TIRE_BR].pwm = 0; 01038 motor[TIRE_BR].dir = FREE; 01039 motor[TIRE_BL].pwm = 20; 01040 motor[TIRE_BL].dir = FOR; 01041 mtc2=true; 01042 break; 01043 case 2: 01044 motor[TIRE_FR].pwm = 30; 01045 motor[TIRE_FR].dir = BACK; 01046 motor[TIRE_FL].pwm = 0; 01047 motor[TIRE_FL].dir = FREE; 01048 motor[TIRE_BR].pwm = 0; 01049 motor[TIRE_BR].dir = FREE; 01050 motor[TIRE_BL].pwm = 30; 01051 motor[TIRE_BL].dir = FOR; 01052 mtc2=true; 01053 break; 01054 } 01055 if(g[2]!=98&&mtc2==true) { 01056 switch(g[2]) { 01057 case 0: 01058 mtc2=false; 01059 break; 01060 case 255: 01061 motor[TIRE_FL].pwm += 5; 01062 mtc2=false; 01063 break; 01064 case 253: 01065 motor[TIRE_FL].pwm += 10; 01066 mtc2=false; 01067 break; 01068 case 254: 01069 motor[TIRE_FL].pwm += 20; 01070 motor[TIRE_BL].pwm = 0; 01071 mtc2=false; 01072 break; 01073 case 1: 01074 motor[TIRE_BL].pwm += 5; 01075 mtc2=false; 01076 break; 01077 case 3: 01078 motor[TIRE_BL].pwm += 10; 01079 mtc2=false; 01080 break; 01081 case 2: 01082 motor[TIRE_BL].pwm += 20; 01083 motor[TIRE_FL].pwm = 0; 01084 mtc2=false; 01085 break; 01086 } 01087 } 01088 } else if(mode==41) { 01089 motor[TIRE_FR].pwm = 0; 01090 motor[TIRE_FR].dir = FREE; 01091 motor[TIRE_FL].pwm = 15; 01092 motor[TIRE_FL].dir = BACK; 01093 motor[TIRE_BR].pwm = 15; 01094 motor[TIRE_BR].dir = FOR; 01095 motor[TIRE_BL].pwm = 0; 01096 motor[TIRE_BL].dir = FREE; 01097 if(count>5000&&g[1]==0) { 01098 count=0; 01099 mode=42; 01100 } 01101 } else if(mode==42) { 01102 motor[TIRE_FR].pwm = 15; 01103 motor[TIRE_FR].dir = FOR; 01104 motor[TIRE_FL].pwm = 15; 01105 motor[TIRE_FL].dir = FOR; 01106 motor[TIRE_BR].pwm = 15; 01107 motor[TIRE_BR].dir = BACK; 01108 motor[TIRE_BL].pwm = 15; 01109 motor[TIRE_BL].dir = BACK; 01110 if(count>5000&&g[1]==0) { 01111 count=0; 01112 mode=50; 01113 } 01114 } else if(mode==50) { 01115 switch(g[1]) { 01116 case 98: 01117 motor[TIRE_FR].pwm = 30; 01118 motor[TIRE_FR].dir = FOR; 01119 motor[TIRE_FL].pwm = 30; 01120 motor[TIRE_FL].dir = BACK; 01121 motor[TIRE_BR].pwm = 30; 01122 motor[TIRE_BR].dir = FOR; 01123 motor[TIRE_BL].pwm = 30; 01124 motor[TIRE_BL].dir = BACK; 01125 if(count>100000) { 01126 cross++; 01127 count=0; 01128 } 01129 mtc=true; 01130 if(cross==1) { 01131 mode=51; 01132 count=0; 01133 } 01134 break; 01135 case 0: 01136 motor[TIRE_FR].pwm = 60; 01137 motor[TIRE_FR].dir = FOR; 01138 motor[TIRE_FL].pwm = 60; 01139 motor[TIRE_FL].dir = BACK; 01140 motor[TIRE_BR].pwm = 60; 01141 motor[TIRE_BR].dir = FOR; 01142 motor[TIRE_BL].pwm = 60; 01143 motor[TIRE_BL].dir = BACK; 01144 mtc=true; 01145 break; 01146 case 255: 01147 motor[TIRE_FR].pwm = 60; 01148 motor[TIRE_FR].dir = FOR; 01149 motor[TIRE_FL].pwm = 40; 01150 motor[TIRE_FL].dir = BACK; 01151 motor[TIRE_BR].pwm = 40; 01152 motor[TIRE_BR].dir = FOR; 01153 motor[TIRE_BL].pwm = 60; 01154 motor[TIRE_BL].dir = BACK; 01155 mtc=true; 01156 break; 01157 case 253: 01158 motor[TIRE_FR].pwm = 20; 01159 motor[TIRE_FR].dir = FOR; 01160 motor[TIRE_FL].pwm = 0; 01161 motor[TIRE_FL].dir = FREE; 01162 motor[TIRE_BR].pwm = 0; 01163 motor[TIRE_BR].dir = FREE; 01164 motor[TIRE_BL].pwm = 20; 01165 motor[TIRE_BL].dir = BACK; 01166 mtc=true; 01167 break; 01168 case 254: 01169 motor[TIRE_FR].pwm = 30; 01170 motor[TIRE_FR].dir = FOR; 01171 motor[TIRE_FL].pwm = 0; 01172 motor[TIRE_FL].dir = FREE; 01173 motor[TIRE_BR].pwm = 0; 01174 motor[TIRE_BR].dir = FREE; 01175 motor[TIRE_BL].pwm = 30; 01176 motor[TIRE_BL].dir = BACK; 01177 mtc=true; 01178 break; 01179 case 1: 01180 motor[TIRE_FR].pwm = 40; 01181 motor[TIRE_FR].dir = FOR; 01182 motor[TIRE_FL].pwm = 60; 01183 motor[TIRE_FL].dir = BACK; 01184 motor[TIRE_BR].pwm = 60; 01185 motor[TIRE_BR].dir = FOR; 01186 motor[TIRE_BL].pwm = 40; 01187 motor[TIRE_BL].dir = BACK; 01188 mtc=true; 01189 break; 01190 case 3: 01191 motor[TIRE_FR].pwm = 0; 01192 motor[TIRE_FR].dir = FREE; 01193 motor[TIRE_FL].pwm = 20; 01194 motor[TIRE_FL].dir = BACK; 01195 motor[TIRE_BR].pwm = 20; 01196 motor[TIRE_BR].dir = FOR; 01197 motor[TIRE_BL].pwm = 0; 01198 motor[TIRE_BL].dir = FREE; 01199 mtc=true; 01200 break; 01201 case 2: 01202 motor[TIRE_FR].pwm = 0; 01203 motor[TIRE_FR].dir = FREE; 01204 motor[TIRE_FL].pwm = 30; 01205 motor[TIRE_FL].dir = BACK; 01206 motor[TIRE_BR].pwm = 30; 01207 motor[TIRE_BR].dir = FOR; 01208 motor[TIRE_BL].pwm = 0; 01209 motor[TIRE_BL].dir = FREE; 01210 mtc=true; 01211 break; 01212 } 01213 if(g[1]!=98&&mtc==true) { 01214 switch(g[0]) { 01215 case 0: 01216 mtc=false; 01217 break; 01218 case 255: 01219 motor[TIRE_FL].pwm += 5; 01220 mtc=false; 01221 break; 01222 case 253: 01223 motor[TIRE_FL].pwm += 10; 01224 mtc=false; 01225 break; 01226 case 254: 01227 motor[TIRE_FL].pwm += 20; 01228 motor[TIRE_FR].pwm = 0; 01229 mtc=false; 01230 break; 01231 case 1: 01232 motor[TIRE_FR].pwm += 5; 01233 mtc=false; 01234 break; 01235 case 3: 01236 motor[TIRE_FR].pwm += 10; 01237 mtc=false; 01238 break; 01239 case 2: 01240 motor[TIRE_FR].pwm += 20; 01241 motor[TIRE_FL].pwm = 0; 01242 mtc=false; 01243 break; 01244 } 01245 } 01246 } else if(mode==51) { 01247 motor[TIRE_FR].pwm = 0; 01248 motor[TIRE_FR].dir = FREE; 01249 motor[TIRE_FL].pwm = 15; 01250 motor[TIRE_FL].dir = BACK; 01251 motor[TIRE_BR].pwm = 15; 01252 motor[TIRE_BR].dir = FOR; 01253 motor[TIRE_BL].pwm = 0; 01254 motor[TIRE_BL].dir = FREE; 01255 if(count>5000&&g[3]==0) { 01256 count=0; 01257 mode=52; 01258 } 01259 } else if(mode==52) { 01260 motor[TIRE_FR].pwm = 15; 01261 motor[TIRE_FR].dir = BACK; 01262 motor[TIRE_FL].pwm = 15; 01263 motor[TIRE_FL].dir = FOR; 01264 motor[TIRE_BR].pwm = 15; 01265 motor[TIRE_BR].dir = BACK; 01266 motor[TIRE_BL].pwm = 15; 01267 motor[TIRE_BL].dir = FOR; 01268 if(count>5000&&g[3]==0) { 01269 count=0; 01270 cross=0; 01271 mode=60; 01272 } 01273 } else if(mode==60) { 01274 switch(g[3]) { 01275 case 98: 01276 motor[TIRE_FR].pwm = 30; 01277 motor[TIRE_FR].dir = BACK; 01278 motor[TIRE_FL].pwm = 30; 01279 motor[TIRE_FL].dir = BACK; 01280 motor[TIRE_BR].pwm = 30; 01281 motor[TIRE_BR].dir = FOR; 01282 motor[TIRE_BL].pwm = 30; 01283 motor[TIRE_BL].dir = FOR; 01284 if(count>50000) { 01285 cross++; 01286 count=0; 01287 } 01288 mtc2=true; 01289 if(cross==2) { 01290 mode=61; 01291 count=0; 01292 cross=0; 01293 } 01294 break; 01295 case 0: 01296 motor[TIRE_FR].pwm = 60; 01297 motor[TIRE_FR].dir = BACK; 01298 motor[TIRE_FL].pwm = 60; 01299 motor[TIRE_FL].dir = BACK; 01300 motor[TIRE_BR].pwm = 60; 01301 motor[TIRE_BR].dir = FOR; 01302 motor[TIRE_BL].pwm = 60; 01303 motor[TIRE_BL].dir = FOR; 01304 mtc2=true; 01305 break; 01306 case 255: 01307 motor[TIRE_FR].pwm = 40; 01308 motor[TIRE_FR].dir = BACK; 01309 motor[TIRE_FL].pwm = 60; 01310 motor[TIRE_FL].dir = BACK; 01311 motor[TIRE_BR].pwm = 60; 01312 motor[TIRE_BR].dir = FOR; 01313 motor[TIRE_BL].pwm = 40; 01314 motor[TIRE_BL].dir = FOR; 01315 mtc2=true; 01316 break; 01317 case 253: 01318 motor[TIRE_FR].pwm = 0; 01319 motor[TIRE_FR].dir = FREE; 01320 motor[TIRE_FL].pwm = 20; 01321 motor[TIRE_FL].dir = BACK; 01322 motor[TIRE_BR].pwm = 20; 01323 motor[TIRE_BR].dir = FOR; 01324 motor[TIRE_BL].pwm = 0; 01325 motor[TIRE_BL].dir = FREE; 01326 mtc2=true; 01327 break; 01328 case 254: 01329 motor[TIRE_FR].pwm = 0; 01330 motor[TIRE_FR].dir = FREE; 01331 motor[TIRE_FL].pwm = 30; 01332 motor[TIRE_FL].dir = BACK; 01333 motor[TIRE_BR].pwm = 30; 01334 motor[TIRE_BR].dir = FOR; 01335 motor[TIRE_BL].pwm = 0; 01336 motor[TIRE_BL].dir = FREE; 01337 mtc2=true; 01338 break; 01339 case 1: 01340 motor[TIRE_FR].pwm = 60; 01341 motor[TIRE_FR].dir = BACK; 01342 motor[TIRE_FL].pwm = 40; 01343 motor[TIRE_FL].dir = BACK; 01344 motor[TIRE_BR].pwm = 40; 01345 motor[TIRE_BR].dir = FOR; 01346 motor[TIRE_BL].pwm = 60; 01347 motor[TIRE_BL].dir = FOR; 01348 mtc2=true; 01349 break; 01350 case 3: 01351 motor[TIRE_FR].pwm = 20; 01352 motor[TIRE_FR].dir = BACK; 01353 motor[TIRE_FL].pwm = 0; 01354 motor[TIRE_FL].dir = FREE; 01355 motor[TIRE_BR].pwm = 0; 01356 motor[TIRE_BR].dir = FREE; 01357 motor[TIRE_BL].pwm = 20; 01358 motor[TIRE_BL].dir = FOR; 01359 mtc2=true; 01360 break; 01361 case 2: 01362 motor[TIRE_FR].pwm = 30; 01363 motor[TIRE_FR].dir = BACK; 01364 motor[TIRE_FL].pwm = 0; 01365 motor[TIRE_FL].dir = FREE; 01366 motor[TIRE_BR].pwm = 0; 01367 motor[TIRE_BR].dir = FREE; 01368 motor[TIRE_BL].pwm = 30; 01369 motor[TIRE_BL].dir = FOR; 01370 mtc2=true; 01371 break; 01372 } 01373 if(g[2]!=98&&mtc2==true) { 01374 switch(g[2]) { 01375 case 0: 01376 mtc2=false; 01377 break; 01378 case 255: 01379 motor[TIRE_FL].pwm += 5; 01380 mtc2=false; 01381 break; 01382 case 253: 01383 motor[TIRE_FL].pwm += 10; 01384 mtc2=false; 01385 break; 01386 case 254: 01387 motor[TIRE_FL].pwm += 20; 01388 motor[TIRE_BL].pwm = 0; 01389 mtc2=false; 01390 break; 01391 case 1: 01392 motor[TIRE_BL].pwm += 5; 01393 mtc2=false; 01394 break; 01395 case 3: 01396 motor[TIRE_BL].pwm += 10; 01397 mtc2=false; 01398 break; 01399 case 2: 01400 motor[TIRE_BL].pwm += 20; 01401 motor[TIRE_FL].pwm = 0; 01402 mtc2=false; 01403 break; 01404 } 01405 } 01406 } else if(mode==61) { 01407 motor[TIRE_FR].pwm = 0; 01408 motor[TIRE_FR].dir = FREE; 01409 motor[TIRE_FL].pwm = 15; 01410 motor[TIRE_FL].dir = BACK; 01411 motor[TIRE_BR].pwm = 15; 01412 motor[TIRE_BR].dir = FOR; 01413 motor[TIRE_BL].pwm = 0; 01414 motor[TIRE_BL].dir = FREE; 01415 if(count>5000&&g[1]==0) { 01416 count=0; 01417 mode=62; 01418 } 01419 } else if(mode==62) { 01420 motor[TIRE_FR].pwm = 15; 01421 motor[TIRE_FR].dir = FOR; 01422 motor[TIRE_FL].pwm = 15; 01423 motor[TIRE_FL].dir = FOR; 01424 motor[TIRE_BR].pwm = 15; 01425 motor[TIRE_BR].dir = BACK; 01426 motor[TIRE_BL].pwm = 15; 01427 motor[TIRE_BL].dir = BACK; 01428 if(count>5000&&g[1]==0) { 01429 count=0; 01430 mode=70; 01431 } 01432 } else if(mode==70) { 01433 switch(g[1]) { 01434 case 98: 01435 motor[TIRE_FR].pwm = 30; 01436 motor[TIRE_FR].dir = FOR; 01437 motor[TIRE_FL].pwm = 30; 01438 motor[TIRE_FL].dir = BACK; 01439 motor[TIRE_BR].pwm = 30; 01440 motor[TIRE_BR].dir = FOR; 01441 motor[TIRE_BL].pwm = 30; 01442 motor[TIRE_BL].dir = BACK; 01443 if(count>100000) { 01444 cross++; 01445 count=0; 01446 } 01447 mtc=true; 01448 if(cross==1) { 01449 mode=71; 01450 count=0; 01451 } 01452 break; 01453 case 0: 01454 motor[TIRE_FR].pwm = 60; 01455 motor[TIRE_FR].dir = FOR; 01456 motor[TIRE_FL].pwm = 60; 01457 motor[TIRE_FL].dir = BACK; 01458 motor[TIRE_BR].pwm = 60; 01459 motor[TIRE_BR].dir = FOR; 01460 motor[TIRE_BL].pwm = 60; 01461 motor[TIRE_BL].dir = BACK; 01462 mtc=true; 01463 break; 01464 case 255: 01465 motor[TIRE_FR].pwm = 60; 01466 motor[TIRE_FR].dir = FOR; 01467 motor[TIRE_FL].pwm = 40; 01468 motor[TIRE_FL].dir = BACK; 01469 motor[TIRE_BR].pwm = 40; 01470 motor[TIRE_BR].dir = FOR; 01471 motor[TIRE_BL].pwm = 60; 01472 motor[TIRE_BL].dir = BACK; 01473 mtc=true; 01474 break; 01475 case 253: 01476 motor[TIRE_FR].pwm = 20; 01477 motor[TIRE_FR].dir = FOR; 01478 motor[TIRE_FL].pwm = 0; 01479 motor[TIRE_FL].dir = FREE; 01480 motor[TIRE_BR].pwm = 0; 01481 motor[TIRE_BR].dir = FREE; 01482 motor[TIRE_BL].pwm = 20; 01483 motor[TIRE_BL].dir = BACK; 01484 mtc=true; 01485 break; 01486 case 254: 01487 motor[TIRE_FR].pwm = 30; 01488 motor[TIRE_FR].dir = FOR; 01489 motor[TIRE_FL].pwm = 0; 01490 motor[TIRE_FL].dir = FREE; 01491 motor[TIRE_BR].pwm = 0; 01492 motor[TIRE_BR].dir = FREE; 01493 motor[TIRE_BL].pwm = 30; 01494 motor[TIRE_BL].dir = BACK; 01495 mtc=true; 01496 break; 01497 case 1: 01498 motor[TIRE_FR].pwm = 40; 01499 motor[TIRE_FR].dir = FOR; 01500 motor[TIRE_FL].pwm = 60; 01501 motor[TIRE_FL].dir = BACK; 01502 motor[TIRE_BR].pwm = 60; 01503 motor[TIRE_BR].dir = FOR; 01504 motor[TIRE_BL].pwm = 40; 01505 motor[TIRE_BL].dir = BACK; 01506 mtc=true; 01507 break; 01508 case 3: 01509 motor[TIRE_FR].pwm = 0; 01510 motor[TIRE_FR].dir = FREE; 01511 motor[TIRE_FL].pwm = 20; 01512 motor[TIRE_FL].dir = BACK; 01513 motor[TIRE_BR].pwm = 20; 01514 motor[TIRE_BR].dir = FOR; 01515 motor[TIRE_BL].pwm = 0; 01516 motor[TIRE_BL].dir = FREE; 01517 mtc=true; 01518 break; 01519 case 2: 01520 motor[TIRE_FR].pwm = 0; 01521 motor[TIRE_FR].dir = FREE; 01522 motor[TIRE_FL].pwm = 30; 01523 motor[TIRE_FL].dir = BACK; 01524 motor[TIRE_BR].pwm = 30; 01525 motor[TIRE_BR].dir = FOR; 01526 motor[TIRE_BL].pwm = 0; 01527 motor[TIRE_BL].dir = FREE; 01528 mtc=true; 01529 break; 01530 } 01531 if(g[1]!=98&&mtc==true) { 01532 switch(g[0]) { 01533 case 0: 01534 mtc=false; 01535 break; 01536 case 255: 01537 motor[TIRE_FL].pwm += 5; 01538 mtc=false; 01539 break; 01540 case 253: 01541 motor[TIRE_FL].pwm += 10; 01542 mtc=false; 01543 break; 01544 case 254: 01545 motor[TIRE_FL].pwm += 20; 01546 motor[TIRE_FR].pwm = 0; 01547 mtc=false; 01548 break; 01549 case 1: 01550 motor[TIRE_FR].pwm += 5; 01551 mtc=false; 01552 break; 01553 case 3: 01554 motor[TIRE_FR].pwm += 10; 01555 mtc=false; 01556 break; 01557 case 2: 01558 motor[TIRE_FR].pwm += 20; 01559 motor[TIRE_FL].pwm = 0; 01560 mtc=false; 01561 break; 01562 } 01563 } 01564 } else if(mode==71) { 01565 motor[TIRE_FR].pwm = 15; 01566 motor[TIRE_FR].dir = FOR; 01567 motor[TIRE_FL].pwm = 15; 01568 motor[TIRE_FL].dir = BACK; 01569 motor[TIRE_BR].pwm = 15; 01570 motor[TIRE_BR].dir = FOR; 01571 motor[TIRE_BL].pwm = 15; 01572 motor[TIRE_BL].dir = BACK; 01573 if(count>5000&&g[3]==0) { 01574 count=0; 01575 mode=72; 01576 } 01577 } else if(mode==72) { 01578 motor[TIRE_FR].pwm = 15; 01579 motor[TIRE_FR].dir = BACK; 01580 motor[TIRE_FL].pwm = 15; 01581 motor[TIRE_FL].dir = FOR; 01582 motor[TIRE_BR].pwm = 15; 01583 motor[TIRE_BR].dir = BACK; 01584 motor[TIRE_BL].pwm = 15; 01585 motor[TIRE_BL].dir = FOR; 01586 count++; 01587 if(count>50000&&g[3]==0) { 01588 count=0; 01589 cross=0; 01590 mode=80; 01591 } 01592 } else if(mode==80) { 01593 if(count>=400000) { 01594 mode=100; 01595 } 01596 motor[TIRE_FR].pwm = 40; 01597 motor[TIRE_FR].dir = FOR; 01598 motor[TIRE_FL].pwm = 40; 01599 motor[TIRE_FL].dir = BACK; 01600 motor[TIRE_BR].pwm = 40; 01601 motor[TIRE_BR].dir = FOR; 01602 motor[TIRE_BL].pwm = 40; 01603 motor[TIRE_BL].dir = BACK; 01604 } else if(mode==100) { 01605 motor[TIRE_FR].pwm = 0; 01606 motor[TIRE_FR].dir = BRAKE; 01607 motor[TIRE_FL].pwm = 0; 01608 motor[TIRE_FL].dir = BRAKE; 01609 motor[TIRE_BR].pwm = 0; 01610 motor[TIRE_BR].dir = BRAKE; 01611 motor[TIRE_BL].pwm = 0; 01612 motor[TIRE_BL].dir = BRAKE; 01613 } else { 01614 motor[TIRE_FR].pwm = 0; 01615 motor[TIRE_FR].dir = BRAKE; 01616 motor[TIRE_FL].pwm = 0; 01617 motor[TIRE_FL].dir = BRAKE; 01618 motor[TIRE_BR].pwm = 0; 01619 motor[TIRE_BR].dir = BRAKE; 01620 motor[TIRE_BL].pwm = 0; 01621 motor[TIRE_BL].dir = BRAKE; 01622 } 01623 01624 } 01625 01626 #endif 01627 01628 #if USE_PROCESS_NUM>3 01629 static void Process3() 01630 { 01631 01632 } 01633 #endif 01634 01635 #if USE_PROCESS_NUM>4 01636 static void Process4() 01637 { 01638 01639 } 01640 #endif 01641 01642 #if USE_PROCESS_NUM>5 01643 static void Process5() 01644 { 01645 01646 } 01647 #endif 01648 01649 #if USE_PROCESS_NUM>6 01650 static void Process6() 01651 { 01652 01653 } 01654 #endif 01655 01656 #if USE_PROCESS_NUM>7 01657 static void Process7() 01658 { 01659 01660 } 01661 #endif 01662 01663 #if USE_PROCESS_NUM>8 01664 static void Process8() 01665 { 01666 01667 } 01668 #endif 01669 01670 #if USE_PROCESS_NUM>9 01671 static void Process9() 01672 { 01673 01674 } 01675 #endif 01676 #endif 01677 #pragma endregion PROCESS 01678 01679 static void AllActuatorReset() 01680 { 01681 01682 #ifdef USE_SOLENOID 01683 solenoid.all = ALL_SOLENOID_OFF; 01684 #endif 01685 01686 #ifdef USE_MOTOR 01687 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 01688 motor[i].dir = FREE; 01689 motor[i].pwm = 0; 01690 } 01691 #endif 01692 } 01693 01694 /* 01695 void BuzzerTimer_func() 01696 { 01697 buzzer = !buzzer; 01698 //LED_DEBUG0 = !LED_DEBUG0; 01699 } 01700 */ 01701 void TapeLedEms_func() 01702 { 01703 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 01704 } 01705 01706 #pragma region USER-DEFINED-FUNCTIONS 01707 01708 01709 #pragma endregion
Generated on Fri Jul 15 2022 02:00:19 by
1.7.2