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