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