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