大季 矢花
/
MainBoard2019_Master_9_22
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 /* 00110 switch(led) { 00111 case 1: 00112 POTENTIOMETER::dio[0]= 1; 00113 POTENTIOMETER::dio[1]= 0; 00114 POTENTIOMETER::dio[2]= 0; 00115 POTENTIOMETER::dio[3]= 0; 00116 break; 00117 case 2: 00118 POTENTIOMETER::dio[0]= 0; 00119 POTENTIOMETER::dio[1]= 1; 00120 POTENTIOMETER::dio[2]= 0; 00121 POTENTIOMETER::dio[3]= 0; 00122 break; 00123 case 3: 00124 POTENTIOMETER::dio[0]= 1; 00125 POTENTIOMETER::dio[1]= 1; 00126 POTENTIOMETER::dio[2]= 0; 00127 POTENTIOMETER::dio[3]= 0; 00128 break; 00129 case 4: 00130 POTENTIOMETER::dio[0]= 0; 00131 POTENTIOMETER::dio[1]= 0; 00132 POTENTIOMETER::dio[2]= 1; 00133 POTENTIOMETER::dio[3]= 0; 00134 break; 00135 } 00136 */ 00137 if(led/8>=1) { 00138 POTENTIOMETER::dio[3]= 1; 00139 led=led%8; 00140 } else { 00141 POTENTIOMETER::dio[3]= 0; 00142 } 00143 00144 if(led/4>=1) { 00145 POTENTIOMETER::dio[2]= 1; 00146 led=led%4; 00147 } else { 00148 POTENTIOMETER::dio[2]= 0; 00149 } 00150 00151 if(led/2>=1) { 00152 POTENTIOMETER::dio[1]= 1; 00153 led=led%2; 00154 } else { 00155 POTENTIOMETER::dio[1]= 0; 00156 } 00157 00158 if(led/1>=1) { 00159 POTENTIOMETER::dio[0]= 1; 00160 led=0; 00161 } else { 00162 POTENTIOMETER::dio[0]= 0; 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 zyougedo; 00171 bool dz1=true; 00172 bool dz1i=false; 00173 bool dz2=true; 00174 bool dz2i=false; 00175 bool dz3=true; 00176 bool dz3i=false; 00177 bool dz4=true; 00178 bool dz4i=false; 00179 bool zone; 00180 bool through=false; 00181 bool counts=false; 00182 bool mtc=false; 00183 bool mtc2 = false; 00184 00185 bool start_flag = true; 00186 00187 int mode =1; 00188 int cross=0;//十字数える用 00189 int cros=0; 00190 int count=100000;//wait代替え 00191 00192 ///*********PWM調整用*********/// 00193 int fast =60; 00194 int normal = 40; 00195 int slow = 20; 00196 00197 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00198 00199 #ifdef USE_SUBPROCESS 00200 #if USE_PROCESS_NUM>0 00201 static void Process0(void); 00202 #endif 00203 #if USE_PROCESS_NUM>1 00204 static void Process1(void); 00205 #endif 00206 #if USE_PROCESS_NUM>2 00207 static void Process2(void); 00208 #endif 00209 #if USE_PROCESS_NUM>3 00210 static void Process3(void); 00211 #endif 00212 #if USE_PROCESS_NUM>4 00213 static void Process4(void); 00214 #endif 00215 #if USE_PROCESS_NUM>5 00216 static void Process5(void); 00217 #endif 00218 #if USE_PROCESS_NUM>6 00219 static void Process6(void); 00220 #endif 00221 #if USE_PROCESS_NUM>7 00222 static void Process7(void); 00223 #endif 00224 #if USE_PROCESS_NUM>8 00225 static void Process8(void); 00226 #endif 00227 #if USE_PROCESS_NUM>9 00228 static void Process9(void); 00229 #endif 00230 #endif 00231 00232 void SystemProcessInitialize() 00233 { 00234 #pragma region USER-DEFINED_VARIABLE_INIT 00235 /*Replace here with the initialization code of your variables.*/ 00236 #pragma endregion USER-DEFINED_VARIABLE_INIT 00237 00238 lock = true; 00239 processChangeComp = true; 00240 current = DEFAULT_PROCESS; 00241 00242 #ifdef USE_SUBPROCESS 00243 #if USE_PROCESS_NUM>0 00244 Process[0] = Process0; 00245 #endif 00246 #if USE_PROCESS_NUM>1 00247 Process[1] = Process1; 00248 #endif 00249 #if USE_PROCESS_NUM>2 00250 Process[2] = Process2; 00251 #endif 00252 #if USE_PROCESS_NUM>3 00253 Process[3] = Process3; 00254 #endif 00255 #if USE_PROCESS_NUM>4 00256 Process[4] = Process4; 00257 #endif 00258 #if USE_PROCESS_NUM>5 00259 Process[5] = Process5; 00260 #endif 00261 #if USE_PROCESS_NUM>6 00262 Process[6] = Process6; 00263 #endif 00264 #if USE_PROCESS_NUM>7 00265 Process[7] = Process7; 00266 #endif 00267 #if USE_PROCESS_NUM>8 00268 Process[8] = Process8; 00269 #endif 00270 #if USE_PROCESS_NUM>9 00271 Process[9] = Process9; 00272 #endif 00273 #endif 00274 } 00275 00276 static void SystemProcessUpdate() 00277 { 00278 #ifdef USE_SUBPROCESS 00279 if(controller->Button.HOME) lock = false; 00280 00281 if(controller->Button.START && processChangeComp) { 00282 current++; 00283 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00284 processChangeComp = false; 00285 } else if(controller->Button.SELECT && processChangeComp) { 00286 current--; 00287 if (current < 0) current = 0; 00288 processChangeComp = false; 00289 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00290 #endif 00291 00292 #ifdef USE_MOTOR 00293 ACTUATORHUB::MOTOR::Motor::Update(motor); 00294 #endif 00295 00296 #ifdef USE_SOLENOID 00297 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00298 #endif 00299 00300 #ifdef USE_RS485 00301 ACTUATORHUB::ActuatorHub::Update(); 00302 #endif 00303 00304 } 00305 00306 int g[8]; 00307 00308 void SystemProcess() 00309 { 00310 SystemProcessInitialize(); 00311 00312 while(1) { 00313 for(int i = 0; i < 8; i++) { 00314 g[i] = LineHub::GetPara(i); 00315 } 00316 if(ThSW) { 00317 Twsh=1; 00318 } else { 00319 Twsh=2; 00320 } 00321 if(StertSW && start_flag) { 00322 start_flag = false; 00323 lock = false; 00324 LedMode(1); 00325 if(ZoneSW) { 00326 mode=1; 00327 current = 2; 00328 } else { 00329 mode=1; 00330 current = 5; 00331 } 00332 } 00333 if(DWSW||DOSW) { 00334 if(DWSW) { 00335 zyouge=true; 00336 } 00337 if(DOSW) { 00338 if(DOLS) { 00339 motor[MOTOR_0].pwm = 100; 00340 motor[MOTOR_0].dir = BRAKE; 00341 } else { 00342 motor[MOTOR_0].pwm = 50; 00343 motor[MOTOR_0].dir = BACK; 00344 } 00345 } 00346 } else if(zyouge==false){ 00347 motor[MOTOR_0].pwm = 100; 00348 motor[MOTOR_0].dir = BRAKE; 00349 } 00350 if(zyouge) { 00351 if(Twsh==1) { 00352 motor[MOTOR_0].pwm = 220; 00353 motor[MOTOR_0].dir = FOR; 00354 if(UPLS) { 00355 motor[MOTOR_0].pwm = 100; 00356 motor[MOTOR_0].dir = BRAKE; 00357 zyouge=false; 00358 } 00359 } 00360 if(Twsh==2) { 00361 motor[MOTOR_0].pwm = 230; 00362 motor[MOTOR_0].dir = FOR; 00363 if(UPLS2) { 00364 motor[MOTOR_0].pwm = 100; 00365 motor[MOTOR_0].dir = BRAKE; 00366 zyouge=false; 00367 } 00368 } 00369 } 00370 if(AIRSW) { 00371 if(SW_flag==false) { 00372 if(Air_flag==false) { 00373 solenoid.solenoid3 = SOLENOID_ON; 00374 Air_flag=true; 00375 SW_flag=true; 00376 } else { 00377 solenoid.solenoid3 = SOLENOID_OFF; 00378 Air_flag=false; 00379 SW_flag=true; 00380 } 00381 } 00382 } else { 00383 SW_flag=false; 00384 } 00385 00386 //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]); 00387 00388 #ifdef USE_MU 00389 controller = CONTROLLER::Controller::GetData(); 00390 #endif 00391 00392 #ifdef USE_ERRORCHECK 00393 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { //& start_flag 00394 CONTROLLER::Controller::DataReset(); 00395 AllActuatorReset(); 00396 lock = true; 00397 } else 00398 #endif 00399 { 00400 00401 #ifdef USE_SUBPROCESS 00402 if(!lock) { 00403 Process[current](); 00404 } else 00405 #endif 00406 { 00407 //ロック時の処理 00408 } 00409 } 00410 /* 00411 //Emergency! 00412 if(!EMG_0 && !EMG_1 && !EMGflag) { 00413 buzzer = 0; 00414 BuzzerTimer.attach(BuzzerTimer_func, 1); 00415 EMGflag = true; 00416 LED_DEBUG0 = 1; 00417 } 00418 if(EMG_0 && EMG_1 && EMGflag) { 00419 buzzer = 1; 00420 BuzzerTimer.detach(); 00421 EMGflag = false; 00422 } 00423 */ 00424 SystemProcessUpdate(); 00425 } 00426 } 00427 00428 #pragma region PROCESS 00429 #ifdef USE_SUBPROCESS 00430 #if USE_PROCESS_NUM>0 00431 static void Process0() 00432 { 00433 ///* 00434 mode=1; 00435 LedMode(1); 00436 if(ThSW) { 00437 Twsh=1; 00438 } else { 00439 Twsh=2; 00440 } 00441 00442 if(StertSW) { 00443 //start_flag == false; 00444 //lock = false; 00445 if(ZoneSW) { 00446 mode=1; 00447 current = 2; 00448 } else { 00449 mode=1; 00450 current = 5; 00451 } 00452 } 00453 if(DWSW) { 00454 zyouge=true; 00455 } 00456 if(DOSW) { 00457 zyougedo=true; 00458 } 00459 if(AIRSW) { 00460 if(SW_flag==false) { 00461 if(Air_flag==false) { 00462 solenoid.solenoid3 = SOLENOID_ON; 00463 Air_flag=true; 00464 SW_flag=true; 00465 } else { 00466 solenoid.solenoid3 = SOLENOID_OFF; 00467 Air_flag=false; 00468 SW_flag=true; 00469 } 00470 } 00471 } else { 00472 SW_flag=false; 00473 } 00474 00475 if(zyouge) { 00476 if(Twsh==1) { 00477 motor[MOTOR_0].pwm = 220; 00478 motor[MOTOR_0].dir = FOR; 00479 if(UPLS) { 00480 motor[MOTOR_0].pwm = 100; 00481 motor[MOTOR_0].dir = BRAKE; 00482 zyouge=false; 00483 } 00484 } 00485 if(Twsh==2) { 00486 motor[MOTOR_0].pwm = 230; 00487 motor[MOTOR_0].dir = FOR; 00488 if(UPLS2) { 00489 motor[MOTOR_0].pwm = 100; 00490 motor[MOTOR_0].dir = BRAKE; 00491 zyouge=false; 00492 } 00493 } 00494 } 00495 if(zyougedo) { 00496 motor[MOTOR_0].pwm = 50; 00497 motor[MOTOR_0].dir = BACK; 00498 if(DOLS) { 00499 motor[MOTOR_0].pwm = 100; 00500 motor[MOTOR_0].dir = BRAKE; 00501 zyougedo=false; 00502 } 00503 } 00504 //*/ 00505 } 00506 00507 #endif 00508 00509 #if USE_PROCESS_NUM>1 00510 static void Process1() 00511 { 00512 start_flag = true; 00513 00514 mtc=false; 00515 mtc2 = false; 00516 LedMode(2); 00517 mode=1; 00518 cross=0; 00519 Twsh=1; 00520 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00521 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00522 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00523 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00524 00525 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00526 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00527 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00528 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00529 00530 if(controller->Button.UP||controller->Button.DOWN) { 00531 00532 if(controller->Button.UP) { 00533 motor[MOTOR_0].pwm =200; 00534 motor[MOTOR_0].dir = FOR; 00535 if(UPLS) { 00536 motor[MOTOR_0].pwm = 0; 00537 motor[MOTOR_0].dir = BRAKE; 00538 } 00539 } 00540 if(controller->Button.DOWN) { 00541 motor[MOTOR_0].pwm = 50; 00542 motor[MOTOR_0].dir = BACK; 00543 } 00544 } else { 00545 motor[MOTOR_0].pwm = 0; 00546 motor[MOTOR_0].dir = BRAKE; 00547 } 00548 00549 if(controller->Button.Y) { 00550 if(dz2==true) { 00551 if(dz2i==false) { 00552 solenoid.solenoid2 = SOLENOID_ON; 00553 dz2i=true; 00554 } else { 00555 solenoid.solenoid2 = SOLENOID_OFF; 00556 dz2i=false; 00557 } 00558 dz2=false; 00559 } 00560 } else { 00561 dz2=true; 00562 } 00563 00564 if(controller->Button.A) { 00565 if(dz3==true) { 00566 if(dz3i==false) { 00567 00568 dz3i=true; 00569 } else { 00570 solenoid.solenoid3 = SOLENOID_OFF; 00571 dz3i=false; 00572 } 00573 dz3=false; 00574 } 00575 } else { 00576 dz3=true; 00577 } 00578 00579 if(controller->Button.B) { 00580 if(dz4==true) { 00581 if(dz4i==false) { 00582 solenoid.solenoid4 = SOLENOID_ON; 00583 dz4i=true; 00584 } else { 00585 solenoid.solenoid4 = SOLENOID_OFF; 00586 dz4i=false; 00587 } 00588 dz4=false; 00589 } 00590 } else { 00591 dz4=true; 00592 } 00593 /* 00594 if(controller->Button.RIGHT){ 00595 motor[MOTOR_1].dir = FOR; 00596 motor[MOTOR_1].pwm = 60; 00597 if (ARM_1){ 00598 motor[MOTOR_1].dir = BRAKE; 00599 } 00600 } 00601 else if(controller->Button.LEFT){ 00602 motor[MOTOR_1].dir = BACK; 00603 motor[MOTOR_1].pwm = 60; 00604 if (ARM_0){ 00605 motor[MOTOR_1].dir = BRAKE; 00606 } 00607 } 00608 */ 00609 } 00610 #endif 00611 00612 #if USE_PROCESS_NUM>2 00613 static void Process2() 00614 { 00615 LedMode(3); 00616 // 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]); 00617 count++; 00618 if(mode==1) {//スタートゾーンから白線検知 00619 motor[TIRE_FR].pwm = normal; 00620 motor[TIRE_FR].dir = BACK; 00621 motor[TIRE_FL].pwm = normal; 00622 motor[TIRE_FL].dir = FOR; 00623 motor[TIRE_BR].pwm = normal; 00624 motor[TIRE_BR].dir = BACK; 00625 motor[TIRE_BL].pwm = normal; 00626 motor[TIRE_BL].dir = FOR; 00627 if(g[2]==0) { 00628 count=100000; 00629 cross=0; 00630 mode=3; 00631 } 00632 } else if(mode==3) { //横ライントレースから縦ライントレースへ 00633 motor[TIRE_FR].pwm = 0; 00634 motor[TIRE_FR].dir = FREE; 00635 motor[TIRE_FL].pwm = normal; 00636 motor[TIRE_FL].dir = FOR; 00637 motor[TIRE_BR].pwm = normal; 00638 motor[TIRE_BR].dir = BACK; 00639 motor[TIRE_BL].pwm = 0; 00640 motor[TIRE_BL].dir = FREE; 00641 if(g[0]==0) { 00642 mode=10; 00643 count=100000; 00644 } 00645 } else if(mode==10) { //縦ライントレース 00646 00647 if(counts==false&&g[2]==0) { 00648 cros++; 00649 counts=true; 00650 } 00651 if(counts==true&&g[2]==99) { 00652 counts=false; 00653 } 00654 00655 if(cros==Twsh) { 00656 mode=11; 00657 count=0; 00658 cros=0; 00659 }//ゆっくりモードに入れなかった時の保険 00660 switch(g[0]) { 00661 case 98: 00662 motor[TIRE_FR].pwm = normal; 00663 motor[TIRE_FR].dir = BACK; 00664 motor[TIRE_FL].pwm = normal; 00665 motor[TIRE_FL].dir = FOR; 00666 motor[TIRE_BR].pwm = normal; 00667 motor[TIRE_BR].dir = BACK; 00668 motor[TIRE_BL].pwm = normal; 00669 motor[TIRE_BL].dir = FOR; 00670 mtc=true; 00671 if(count>80000) { 00672 cross++; 00673 count=0; 00674 } 00675 if(cross==Twsh) { 00676 mode=11; 00677 count=0; 00678 cross=0; 00679 } 00680 break; 00681 case 0: 00682 motor[TIRE_FR].pwm = fast; 00683 motor[TIRE_FR].dir = BACK; 00684 motor[TIRE_FL].pwm = fast; 00685 motor[TIRE_FL].dir = FOR; 00686 motor[TIRE_BR].pwm = fast; 00687 motor[TIRE_BR].dir = BACK; 00688 motor[TIRE_BL].pwm = fast; 00689 motor[TIRE_BL].dir = FOR; 00690 mtc=true; 00691 break; 00692 case 255: 00693 motor[TIRE_FR].pwm = fast; 00694 motor[TIRE_FR].dir = BACK; 00695 motor[TIRE_FL].pwm = slow; 00696 motor[TIRE_FL].dir = FOR; 00697 motor[TIRE_BR].pwm = slow; 00698 motor[TIRE_BR].dir = BACK; 00699 motor[TIRE_BL].pwm = fast; 00700 motor[TIRE_BL].dir = FOR; 00701 mtc=true; 00702 break; 00703 case 253: 00704 motor[TIRE_FR].pwm = slow; 00705 motor[TIRE_FR].dir = BACK; 00706 motor[TIRE_FL].pwm = 0; 00707 motor[TIRE_FL].dir = FREE; 00708 motor[TIRE_BR].pwm = 0; 00709 motor[TIRE_BR].dir = FREE; 00710 motor[TIRE_BL].pwm = slow; 00711 motor[TIRE_BL].dir = FOR; 00712 mtc=true; 00713 break; 00714 case 254: 00715 motor[TIRE_FR].pwm = normal; 00716 motor[TIRE_FR].dir = BACK; 00717 motor[TIRE_FL].pwm = 0; 00718 motor[TIRE_FL].dir = FREE; 00719 motor[TIRE_BR].pwm = 0; 00720 motor[TIRE_BR].dir = FREE; 00721 motor[TIRE_BL].pwm = normal; 00722 motor[TIRE_BL].dir = FOR; 00723 mtc=true; 00724 break; 00725 case 1: 00726 motor[TIRE_FR].pwm = slow; 00727 motor[TIRE_FR].dir = BACK; 00728 motor[TIRE_FL].pwm = fast; 00729 motor[TIRE_FL].dir = FOR; 00730 motor[TIRE_BR].pwm = fast; 00731 motor[TIRE_BR].dir = BACK; 00732 motor[TIRE_BL].pwm = slow; 00733 motor[TIRE_BL].dir = FOR; 00734 mtc=true; 00735 break; 00736 case 3: 00737 motor[TIRE_FR].pwm = 0; 00738 motor[TIRE_FR].dir = FREE; 00739 motor[TIRE_FL].pwm = slow; 00740 motor[TIRE_FL].dir = FOR; 00741 motor[TIRE_BR].pwm = slow; 00742 motor[TIRE_BR].dir = BACK; 00743 motor[TIRE_BL].pwm = 0; 00744 motor[TIRE_BL].dir = FREE; 00745 mtc=true; 00746 break; 00747 case 2: 00748 motor[TIRE_FR].pwm = 0; 00749 motor[TIRE_FR].dir = FREE; 00750 motor[TIRE_FL].pwm = normal; 00751 motor[TIRE_FL].dir = FOR; 00752 motor[TIRE_BR].pwm = normal; 00753 motor[TIRE_BR].dir = BACK; 00754 motor[TIRE_BL].pwm = 0; 00755 motor[TIRE_BL].dir = FREE; 00756 mtc=true; 00757 break; 00758 } 00759 if(g[0]!=98&&mtc==true) { 00760 switch(g[1]) { 00761 case 0: 00762 mtc=false; 00763 break; 00764 case 255: 00765 motor[TIRE_BR].pwm += 5; 00766 mtc=false; 00767 break; 00768 case 253: 00769 motor[TIRE_BR].pwm += 10; 00770 mtc=false; 00771 break; 00772 case 254: 00773 motor[TIRE_BR].pwm += 20; 00774 motor[TIRE_BL].pwm = 0; 00775 mtc=false; 00776 break; 00777 case 1: 00778 motor[TIRE_BL].pwm += 5; 00779 mtc=false; 00780 break; 00781 case 3: 00782 motor[TIRE_BL].pwm += 10; 00783 mtc=false; 00784 break; 00785 case 2: 00786 motor[TIRE_BL].pwm += 20; 00787 motor[TIRE_BR].pwm = 0; 00788 mtc=false; 00789 break; 00790 } 00791 } 00792 } else if(mode==11) { 00793 motor[TIRE_FR].pwm = 0; 00794 motor[TIRE_FR].dir = FREE; 00795 motor[TIRE_FL].pwm = normal; 00796 motor[TIRE_FL].dir = FOR; 00797 motor[TIRE_BR].pwm = normal; 00798 motor[TIRE_BR].dir = BACK; 00799 motor[TIRE_BL].pwm = 0; 00800 motor[TIRE_BL].dir = FREE; 00801 if(g[2]==0) { 00802 count=0; 00803 cross=0; 00804 mode=20; 00805 } 00806 } else if(mode==20) { 00807 00808 if(Twsh==2) { 00809 solenoid.solenoid2 = SOLENOID_ON; 00810 } 00811 if(counts==false&&g[0]==0) { 00812 cros++; 00813 counts=true; 00814 } 00815 if(counts==true&&g[0]==99) { 00816 counts=false; 00817 } 00818 00819 if(cros==2) { 00820 00821 // mode=21; 00822 // count=0; 00823 // cros=0; 00824 }//ゆっくりモードに入れなかった時の保険 00825 00826 switch(g[2]) { 00827 case 98: 00828 motor[TIRE_FR].pwm = normal; 00829 motor[TIRE_FR].dir = FOR; 00830 motor[TIRE_FL].pwm = normal; 00831 motor[TIRE_FL].dir = FOR; 00832 motor[TIRE_BR].pwm = normal; 00833 motor[TIRE_BR].dir = BACK; 00834 motor[TIRE_BL].pwm = normal; 00835 motor[TIRE_BL].dir = BACK; 00836 if(count>80000) { 00837 cross++; 00838 count=0; 00839 } 00840 mtc2=true; 00841 if(cross==2) { 00842 mode=21; 00843 count=0; 00844 cross=0; 00845 } 00846 break; 00847 case 0: 00848 motor[TIRE_FR].pwm = fast; 00849 motor[TIRE_FR].dir = FOR; 00850 motor[TIRE_FL].pwm = fast; 00851 motor[TIRE_FL].dir = FOR; 00852 motor[TIRE_BR].pwm = fast; 00853 motor[TIRE_BR].dir = BACK; 00854 motor[TIRE_BL].pwm = fast; 00855 motor[TIRE_BL].dir = BACK; 00856 mtc2=true; 00857 break; 00858 case 255: 00859 motor[TIRE_FR].pwm = normal; 00860 motor[TIRE_FR].dir = FOR; 00861 motor[TIRE_FL].pwm = fast; 00862 motor[TIRE_FL].dir = FOR; 00863 motor[TIRE_BR].pwm = fast; 00864 motor[TIRE_BR].dir = BACK; 00865 motor[TIRE_BL].pwm = normal; 00866 motor[TIRE_BL].dir = BACK; 00867 mtc2=true; 00868 break; 00869 case 253: 00870 motor[TIRE_FR].pwm = 0; 00871 motor[TIRE_FR].dir = FREE; 00872 motor[TIRE_FL].pwm = slow; 00873 motor[TIRE_FL].dir = FOR; 00874 motor[TIRE_BR].pwm = slow; 00875 motor[TIRE_BR].dir = BACK; 00876 motor[TIRE_BL].pwm = 0; 00877 motor[TIRE_BL].dir = FREE; 00878 mtc2=true; 00879 break; 00880 case 254: 00881 motor[TIRE_FR].pwm = 0; 00882 motor[TIRE_FR].dir = FREE; 00883 motor[TIRE_FL].pwm = normal; 00884 motor[TIRE_FL].dir = FOR; 00885 motor[TIRE_BR].pwm = normal; 00886 motor[TIRE_BR].dir = BACK; 00887 motor[TIRE_BL].pwm = 0; 00888 motor[TIRE_BL].dir = FREE; 00889 mtc2=true; 00890 break; 00891 case 1: 00892 motor[TIRE_FR].pwm = fast; 00893 motor[TIRE_FR].dir = FOR; 00894 motor[TIRE_FL].pwm = slow; 00895 motor[TIRE_FL].dir = FOR; 00896 motor[TIRE_BR].pwm = slow; 00897 motor[TIRE_BR].dir = BACK; 00898 motor[TIRE_BL].pwm = fast; 00899 motor[TIRE_BL].dir = BACK; 00900 mtc2=true; 00901 break; 00902 case 3: 00903 motor[TIRE_FR].pwm = slow; 00904 motor[TIRE_FR].dir = FOR; 00905 motor[TIRE_FL].pwm = 0; 00906 motor[TIRE_FL].dir = FREE; 00907 motor[TIRE_BR].pwm = 0; 00908 motor[TIRE_BR].dir = FREE; 00909 motor[TIRE_BL].pwm = slow; 00910 motor[TIRE_BL].dir = BACK; 00911 mtc2=true; 00912 break; 00913 case 2: 00914 motor[TIRE_FR].pwm = normal; 00915 motor[TIRE_FR].dir = FOR; 00916 motor[TIRE_FL].pwm = 0; 00917 motor[TIRE_FL].dir = FREE; 00918 motor[TIRE_BR].pwm = 0; 00919 motor[TIRE_BR].dir = FREE; 00920 motor[TIRE_BL].pwm = normal; 00921 motor[TIRE_BL].dir = BACK; 00922 mtc2=true; 00923 break; 00924 } 00925 if(g[2]!=98&&mtc2==true) { 00926 switch(g[3]) { 00927 case 0: 00928 mtc2=false; 00929 break; 00930 case 255: 00931 motor[TIRE_BL].pwm += 5; 00932 mtc2=false; 00933 break; 00934 case 253: 00935 motor[TIRE_BL].pwm += 10; 00936 mtc2=false; 00937 break; 00938 case 254: 00939 motor[TIRE_BL].pwm += 20; 00940 motor[TIRE_FL].pwm = 0; 00941 mtc2=false; 00942 break; 00943 case 1: 00944 motor[TIRE_FL].pwm += 5; 00945 mtc2=false; 00946 break; 00947 case 3: 00948 motor[TIRE_FL].pwm += 10; 00949 mtc2=false; 00950 break; 00951 case 2: 00952 motor[TIRE_FL].pwm += 20; 00953 motor[TIRE_BL].pwm = 0; 00954 mtc2=false; 00955 break; 00956 } 00957 } 00958 } else if(mode==21) { 00959 motor[TIRE_FR].pwm = 0; 00960 motor[TIRE_FR].dir = FREE; 00961 motor[TIRE_FL].pwm = normal; 00962 motor[TIRE_FL].dir = FOR; 00963 motor[TIRE_BR].pwm = normal; 00964 motor[TIRE_BR].dir = BACK; 00965 motor[TIRE_BL].pwm = 0; 00966 motor[TIRE_BL].dir = FREE; 00967 if(g[0]==0) { 00968 mode=30; 00969 cros=0; 00970 zyouge=true; 00971 } 00972 } else if(mode==30) { 00973 motor[TIRE_FR].pwm = 62; 00974 motor[TIRE_FR].dir = BACK; 00975 motor[TIRE_FL].pwm = 60; 00976 motor[TIRE_FL].dir = FOR; 00977 motor[TIRE_BR].pwm = 60; 00978 motor[TIRE_BR].dir = BACK; 00979 motor[TIRE_BL].pwm = 62; 00980 motor[TIRE_BL].dir = FOR; 00981 if(g[0]==98) { 00982 mode=31; 00983 } 00984 if(g[3]==0) { 00985 mode=32; 00986 count=0; 00987 cros=0; 00988 }//ゆっくりモードに入れなかった時の保険 00989 /* 00990 if(g[0]==98) { 00991 if(Twsh==2) { 00992 solenoid.solenoid2 = SOLENOID_OFF; 00993 } 00994 mode=31; 00995 count=0; 00996 } 00997 switch(g[0]) { 00998 case 98: 00999 motor[TIRE_FR].pwm = 30; 01000 motor[TIRE_FR].dir = BACK; 01001 motor[TIRE_FL].pwm = 30; 01002 motor[TIRE_FL].dir = FOR; 01003 motor[TIRE_BR].pwm = 30; 01004 motor[TIRE_BR].dir = BACK; 01005 motor[TIRE_BL].pwm = 30; 01006 motor[TIRE_BL].dir = FOR; 01007 if(count>100000) { 01008 cross++; 01009 count=0; 01010 } 01011 mtc=true; 01012 if(cross==1) { 01013 if(Twsh==2) { 01014 solenoid.solenoid2 = SOLENOID_OFF; 01015 } 01016 mode=31; 01017 count=0; 01018 } 01019 break; 01020 case 0: 01021 motor[TIRE_FR].pwm = 80; 01022 motor[TIRE_FR].dir = BACK; 01023 motor[TIRE_FL].pwm = 80; 01024 motor[TIRE_FL].dir = FOR; 01025 motor[TIRE_BR].pwm = 80; 01026 motor[TIRE_BR].dir = BACK; 01027 motor[TIRE_BL].pwm = 80; 01028 motor[TIRE_BL].dir = FOR; 01029 mtc=true; 01030 break; 01031 case 255: 01032 motor[TIRE_FR].pwm = 80; 01033 motor[TIRE_FR].dir = BACK; 01034 motor[TIRE_FL].pwm = 40; 01035 motor[TIRE_FL].dir = FOR; 01036 motor[TIRE_BR].pwm = 40; 01037 motor[TIRE_BR].dir = BACK; 01038 motor[TIRE_BL].pwm = 80; 01039 motor[TIRE_BL].dir = FOR; 01040 mtc=true; 01041 break; 01042 case 253: 01043 motor[TIRE_FR].pwm = 80; 01044 motor[TIRE_FR].dir = BACK; 01045 motor[TIRE_FL].pwm = 0; 01046 motor[TIRE_FL].dir = FREE; 01047 motor[TIRE_BR].pwm = 0; 01048 motor[TIRE_BR].dir = FREE; 01049 motor[TIRE_BL].pwm = 40; 01050 motor[TIRE_BL].dir = FOR; 01051 mtc=true; 01052 break; 01053 case 254: 01054 motor[TIRE_FR].pwm = 50; 01055 motor[TIRE_FR].dir = BACK; 01056 motor[TIRE_FL].pwm = 0; 01057 motor[TIRE_FL].dir = FREE; 01058 motor[TIRE_BR].pwm = 0; 01059 motor[TIRE_BR].dir = FREE; 01060 motor[TIRE_BL].pwm = 50; 01061 motor[TIRE_BL].dir = FOR; 01062 mtc=true; 01063 break; 01064 case 1: 01065 motor[TIRE_FR].pwm = 40; 01066 motor[TIRE_FR].dir = BACK; 01067 motor[TIRE_FL].pwm = 80; 01068 motor[TIRE_FL].dir = FOR; 01069 motor[TIRE_BR].pwm = 80; 01070 motor[TIRE_BR].dir = BACK; 01071 motor[TIRE_BL].pwm = 40; 01072 motor[TIRE_BL].dir = FOR; 01073 mtc=true; 01074 break; 01075 case 3: 01076 motor[TIRE_FR].pwm = 0; 01077 motor[TIRE_FR].dir = FREE; 01078 motor[TIRE_FL].pwm = 40; 01079 motor[TIRE_FL].dir = FOR; 01080 motor[TIRE_BR].pwm = 40; 01081 motor[TIRE_BR].dir = BACK; 01082 motor[TIRE_BL].pwm = 0; 01083 motor[TIRE_BL].dir = FREE; 01084 mtc=true; 01085 break; 01086 case 2: 01087 motor[TIRE_FR].pwm = 0; 01088 motor[TIRE_FR].dir = FREE; 01089 motor[TIRE_FL].pwm = 50; 01090 motor[TIRE_FL].dir = FOR; 01091 motor[TIRE_BR].pwm = 50; 01092 motor[TIRE_BR].dir = BACK; 01093 motor[TIRE_BL].pwm = 0; 01094 motor[TIRE_BL].dir = FREE; 01095 mtc=true; 01096 break; 01097 } 01098 if(g[0]!=98&&mtc==true) { 01099 switch(g[1]) { 01100 case 0: 01101 mtc=false; 01102 break; 01103 case 255: 01104 motor[TIRE_BR].pwm += 5; 01105 mtc=false; 01106 break; 01107 case 253: 01108 motor[TIRE_BR].pwm += 10; 01109 mtc=false; 01110 break; 01111 case 254: 01112 motor[TIRE_BR].pwm += 20; 01113 motor[TIRE_BL].pwm = 0; 01114 mtc=false; 01115 break; 01116 case 1: 01117 motor[TIRE_BL].pwm += 5; 01118 mtc=false; 01119 break; 01120 case 3: 01121 motor[TIRE_BL].pwm += 10; 01122 mtc=false; 01123 break; 01124 case 2: 01125 motor[TIRE_BL].pwm += 20; 01126 motor[TIRE_BR].pwm = 0; 01127 mtc=false; 01128 break; 01129 } 01130 01131 } 01132 } else if(mode==31) { 01133 motor[TIRE_FR].pwm = 15; 01134 motor[TIRE_FR].dir = BACK; 01135 motor[TIRE_FL].pwm = 10; 01136 motor[TIRE_FL].dir = BACK; 01137 motor[TIRE_BR].pwm = 10; 01138 motor[TIRE_BR].dir = FOR; 01139 motor[TIRE_BL].pwm = 15; 01140 motor[TIRE_BL].dir = FOR; 01141 if(g[3]==0) { 01142 count=0; 01143 mode=40; 01144 } 01145 */ 01146 01147 } else if(mode==31) { 01148 motor[TIRE_FR].pwm = 15; 01149 motor[TIRE_FR].dir = BACK; 01150 motor[TIRE_FL].pwm = 15; 01151 motor[TIRE_FL].dir = FOR; 01152 motor[TIRE_BR].pwm = 15; 01153 motor[TIRE_BR].dir = BACK; 01154 motor[TIRE_BL].pwm = 15; 01155 motor[TIRE_BL].dir = FOR; 01156 if(g[3]==0) { 01157 count=0; 01158 mode=32; 01159 } 01160 } else if(mode==32) { 01161 motor[TIRE_FR].pwm = 30; 01162 motor[TIRE_FR].dir = FOR; 01163 motor[TIRE_FL].pwm = 30; 01164 motor[TIRE_FL].dir = BACK; 01165 motor[TIRE_BR].pwm = 30; 01166 motor[TIRE_BR].dir = FOR; 01167 motor[TIRE_BL].pwm = 30; 01168 motor[TIRE_BL].dir = BACK; 01169 if(g[3]==0) { 01170 count=0; 01171 if(Twsh==2) { 01172 solenoid.solenoid2 = SOLENOID_OFF; 01173 } 01174 mode=40; 01175 } 01176 } else if(mode==40) { 01177 if(LIB) { 01178 mode=42; 01179 } 01180 if(counts==false&&g[0]==0) { 01181 cros++; 01182 counts=true; 01183 } 01184 if(counts==true&&g[0]==99) { 01185 counts=false; 01186 } 01187 01188 if(cros==1) { 01189 // mode=41; 01190 //count=0; 01191 //cros=0; 01192 }//ゆっくりモードに入れなかった時の保険 01193 01194 switch(g[3]) { 01195 case 98: 01196 motor[TIRE_FR].pwm = normal; 01197 motor[TIRE_FR].dir = BACK; 01198 motor[TIRE_FL].pwm = normal; 01199 motor[TIRE_FL].dir = BACK; 01200 motor[TIRE_BR].pwm = normal; 01201 motor[TIRE_BR].dir = FOR; 01202 motor[TIRE_BL].pwm = normal; 01203 motor[TIRE_BL].dir = FOR; 01204 if(count>20000) { 01205 cross++; 01206 count=0; 01207 } 01208 mtc2=true; 01209 if(cross==2) { 01210 mode=41; 01211 count=0; 01212 cross=0; 01213 } 01214 break; 01215 case 0: 01216 motor[TIRE_FR].pwm = fast; 01217 motor[TIRE_FR].dir = BACK; 01218 motor[TIRE_FL].pwm = fast; 01219 motor[TIRE_FL].dir = BACK; 01220 motor[TIRE_BR].pwm = fast; 01221 motor[TIRE_BR].dir = FOR; 01222 motor[TIRE_BL].pwm = fast; 01223 motor[TIRE_BL].dir = FOR; 01224 mtc2=true; 01225 break; 01226 case 255: 01227 motor[TIRE_FR].pwm = slow; 01228 motor[TIRE_FR].dir = BACK; 01229 motor[TIRE_FL].pwm = fast; 01230 motor[TIRE_FL].dir = BACK; 01231 motor[TIRE_BR].pwm = fast; 01232 motor[TIRE_BR].dir = FOR; 01233 motor[TIRE_BL].pwm = slow; 01234 motor[TIRE_BL].dir = FOR; 01235 mtc2=true; 01236 break; 01237 case 253: 01238 motor[TIRE_FR].pwm = 0; 01239 motor[TIRE_FR].dir = FREE; 01240 motor[TIRE_FL].pwm = slow; 01241 motor[TIRE_FL].dir = BACK; 01242 motor[TIRE_BR].pwm = slow; 01243 motor[TIRE_BR].dir = FOR; 01244 motor[TIRE_BL].pwm = 0; 01245 motor[TIRE_BL].dir = FREE; 01246 mtc2=true; 01247 break; 01248 case 254: 01249 motor[TIRE_FR].pwm = 0; 01250 motor[TIRE_FR].dir = FREE; 01251 motor[TIRE_FL].pwm = normal; 01252 motor[TIRE_FL].dir = BACK; 01253 motor[TIRE_BR].pwm = normal; 01254 motor[TIRE_BR].dir = FOR; 01255 motor[TIRE_BL].pwm = 0; 01256 motor[TIRE_BL].dir = FREE; 01257 mtc2=true; 01258 break; 01259 case 1: 01260 motor[TIRE_FR].pwm = fast; 01261 motor[TIRE_FR].dir = BACK; 01262 motor[TIRE_FL].pwm = slow; 01263 motor[TIRE_FL].dir = BACK; 01264 motor[TIRE_BR].pwm = slow; 01265 motor[TIRE_BR].dir = FOR; 01266 motor[TIRE_BL].pwm = fast; 01267 motor[TIRE_BL].dir = FOR; 01268 mtc2=true; 01269 break; 01270 case 3: 01271 motor[TIRE_FR].pwm = slow; 01272 motor[TIRE_FR].dir = BACK; 01273 motor[TIRE_FL].pwm = 0; 01274 motor[TIRE_FL].dir = FREE; 01275 motor[TIRE_BR].pwm = 0; 01276 motor[TIRE_BR].dir = FREE; 01277 motor[TIRE_BL].pwm = slow; 01278 motor[TIRE_BL].dir = FOR; 01279 mtc2=true; 01280 break; 01281 case 2: 01282 motor[TIRE_FR].pwm = normal; 01283 motor[TIRE_FR].dir = BACK; 01284 motor[TIRE_FL].pwm = 0; 01285 motor[TIRE_FL].dir = FREE; 01286 motor[TIRE_BR].pwm = 0; 01287 motor[TIRE_BR].dir = FREE; 01288 motor[TIRE_BL].pwm = normal; 01289 motor[TIRE_BL].dir = FOR; 01290 mtc2=true; 01291 break; 01292 } 01293 if(g[2]!=98&&mtc2==true) { 01294 switch(g[2]) { 01295 case 0: 01296 mtc2=false; 01297 break; 01298 case 255: 01299 motor[TIRE_FL].pwm += 5; 01300 mtc2=false; 01301 break; 01302 case 253: 01303 motor[TIRE_FL].pwm += 10; 01304 mtc2=false; 01305 break; 01306 case 254: 01307 motor[TIRE_FL].pwm += 20; 01308 motor[TIRE_BL].pwm = 0; 01309 mtc2=false; 01310 break; 01311 case 1: 01312 motor[TIRE_BL].pwm += 5; 01313 mtc2=false; 01314 break; 01315 case 3: 01316 motor[TIRE_BL].pwm += 10; 01317 mtc2=false; 01318 break; 01319 case 2: 01320 motor[TIRE_BL].pwm += 20; 01321 motor[TIRE_FL].pwm = 0; 01322 mtc2=false; 01323 break; 01324 } 01325 } 01326 } else if(mode==41) { 01327 motor[TIRE_FR].pwm = 0; 01328 motor[TIRE_FR].dir = FREE; 01329 motor[TIRE_FL].pwm = 30; 01330 motor[TIRE_FL].dir = BACK; 01331 motor[TIRE_BR].pwm = 30; 01332 motor[TIRE_BR].dir = FOR; 01333 motor[TIRE_BL].pwm = 0; 01334 motor[TIRE_BL].dir = FREE; 01335 if(g[1]==0) { 01336 count=0; 01337 mode=70; 01338 } 01339 } else if(mode==42) { 01340 motor[TIRE_FR].pwm = 20; 01341 motor[TIRE_FR].dir = FOR; 01342 motor[TIRE_FL].pwm = 0; 01343 motor[TIRE_FL].dir = FREE; 01344 motor[TIRE_BR].pwm = 0; 01345 motor[TIRE_BR].dir = FREE; 01346 motor[TIRE_BL].pwm = 20; 01347 motor[TIRE_BL].dir = BACK; 01348 if(g[1]==0) { 01349 count=0; 01350 mode=70; 01351 } 01352 } else if(mode==70) { 01353 switch(g[1]) { 01354 case 99: 01355 if(count>=100000) { 01356 mode=100; 01357 } 01358 break; 01359 case 98: 01360 motor[TIRE_FR].pwm = normal; 01361 motor[TIRE_FR].dir = FOR; 01362 motor[TIRE_FL].pwm = normal; 01363 motor[TIRE_FL].dir = BACK; 01364 motor[TIRE_BR].pwm = normal; 01365 motor[TIRE_BR].dir = FOR; 01366 motor[TIRE_BL].pwm = normal; 01367 motor[TIRE_BL].dir = BACK; 01368 break; 01369 case 0: 01370 motor[TIRE_FR].pwm = 80; 01371 motor[TIRE_FR].dir = FOR; 01372 motor[TIRE_FL].pwm = 80; 01373 motor[TIRE_FL].dir = BACK; 01374 motor[TIRE_BR].pwm = 80; 01375 motor[TIRE_BR].dir = FOR; 01376 motor[TIRE_BL].pwm = 80; 01377 motor[TIRE_BL].dir = BACK; 01378 mtc=true; 01379 break; 01380 case 255: 01381 motor[TIRE_FR].pwm = fast; 01382 motor[TIRE_FR].dir = FOR; 01383 motor[TIRE_FL].pwm = slow; 01384 motor[TIRE_FL].dir = BACK; 01385 motor[TIRE_BR].pwm = slow; 01386 motor[TIRE_BR].dir = FOR; 01387 motor[TIRE_BL].pwm = fast; 01388 motor[TIRE_BL].dir = BACK; 01389 mtc=true; 01390 break; 01391 case 253: 01392 motor[TIRE_FR].pwm = slow; 01393 motor[TIRE_FR].dir = FOR; 01394 motor[TIRE_FL].pwm = 0; 01395 motor[TIRE_FL].dir = FREE; 01396 motor[TIRE_BR].pwm = 0; 01397 motor[TIRE_BR].dir = FREE; 01398 motor[TIRE_BL].pwm = slow; 01399 motor[TIRE_BL].dir = BACK; 01400 mtc=true; 01401 break; 01402 case 254: 01403 motor[TIRE_FR].pwm = normal; 01404 motor[TIRE_FR].dir = FOR; 01405 motor[TIRE_FL].pwm = 0; 01406 motor[TIRE_FL].dir = FREE; 01407 motor[TIRE_BR].pwm = 0; 01408 motor[TIRE_BR].dir = FREE; 01409 motor[TIRE_BL].pwm = normal; 01410 motor[TIRE_BL].dir = BACK; 01411 mtc=true; 01412 break; 01413 case 1: 01414 motor[TIRE_FR].pwm = slow; 01415 motor[TIRE_FR].dir = FOR; 01416 motor[TIRE_FL].pwm = fast; 01417 motor[TIRE_FL].dir = BACK; 01418 motor[TIRE_BR].pwm = fast; 01419 motor[TIRE_BR].dir = FOR; 01420 motor[TIRE_BL].pwm = slow; 01421 motor[TIRE_BL].dir = BACK; 01422 mtc=true; 01423 break; 01424 case 3: 01425 motor[TIRE_FR].pwm = 0; 01426 motor[TIRE_FR].dir = FREE; 01427 motor[TIRE_FL].pwm = slow; 01428 motor[TIRE_FL].dir = BACK; 01429 motor[TIRE_BR].pwm = slow; 01430 motor[TIRE_BR].dir = FOR; 01431 motor[TIRE_BL].pwm = 0; 01432 motor[TIRE_BL].dir = FREE; 01433 mtc=true; 01434 break; 01435 case 2: 01436 motor[TIRE_FR].pwm = 0; 01437 motor[TIRE_FR].dir = FREE; 01438 motor[TIRE_FL].pwm = normal; 01439 motor[TIRE_FL].dir = BACK; 01440 motor[TIRE_BR].pwm = normal; 01441 motor[TIRE_BR].dir = FOR; 01442 motor[TIRE_BL].pwm = 0; 01443 motor[TIRE_BL].dir = FREE; 01444 mtc=true; 01445 break; 01446 } 01447 if(g[1]!=98&&mtc==true) { 01448 switch(g[0]) { 01449 case 0: 01450 mtc=false; 01451 break; 01452 case 255: 01453 motor[TIRE_FL].pwm += 5; 01454 mtc=false; 01455 break; 01456 case 253: 01457 motor[TIRE_FL].pwm += 10; 01458 mtc=false; 01459 break; 01460 case 254: 01461 motor[TIRE_FL].pwm += 20; 01462 motor[TIRE_FR].pwm = 0; 01463 mtc=false; 01464 break; 01465 case 1: 01466 motor[TIRE_FR].pwm += 5; 01467 mtc=false; 01468 break; 01469 case 3: 01470 motor[TIRE_FR].pwm += 10; 01471 mtc=false; 01472 break; 01473 case 2: 01474 motor[TIRE_FR].pwm += 20; 01475 motor[TIRE_FL].pwm = 0; 01476 mtc=false; 01477 break; 01478 } 01479 } 01480 } else if(mode==100) { 01481 motor[TIRE_FR].pwm = 20; 01482 motor[TIRE_FR].dir = FOR; 01483 motor[TIRE_FL].pwm = 20; 01484 motor[TIRE_FL].dir = BACK; 01485 motor[TIRE_BR].pwm = 23; 01486 motor[TIRE_BR].dir = FOR; 01487 motor[TIRE_BL].pwm = 20; 01488 motor[TIRE_BL].dir = BACK; 01489 } else { 01490 motor[TIRE_FR].pwm = 0; 01491 motor[TIRE_FR].dir = BRAKE; 01492 motor[TIRE_FL].pwm = 0; 01493 motor[TIRE_FL].dir = BRAKE; 01494 motor[TIRE_BR].pwm = 0; 01495 motor[TIRE_BR].dir = BRAKE; 01496 motor[TIRE_BL].pwm = 0; 01497 motor[TIRE_BL].dir = BRAKE; 01498 } 01499 } 01500 #endif 01501 01502 #if USE_PROCESS_NUM>3 01503 static void Process3() 01504 { 01505 01506 } 01507 #endif 01508 01509 #if USE_PROCESS_NUM>4 01510 static void Process4() 01511 { 01512 LedMode(4); 01513 count++; 01514 if(mode==1) {//スタートゾーンから白線検知 01515 motor[TIRE_FR].pwm = 40; 01516 motor[TIRE_FR].dir = FOR; 01517 motor[TIRE_FL].pwm = 40; 01518 motor[TIRE_FL].dir = BACK; 01519 motor[TIRE_BR].pwm = 40; 01520 motor[TIRE_BR].dir = FOR; 01521 motor[TIRE_BL].pwm = 45; 01522 motor[TIRE_BL].dir = BACK; 01523 if(g[2]==0) { 01524 count=100000; 01525 cross=0; 01526 mode=3; 01527 } 01528 } else if(mode==3) { //横ライントレースから縦ライントレースへ 01529 motor[TIRE_FR].pwm = 40; 01530 motor[TIRE_FR].dir = FOR; 01531 motor[TIRE_FL].pwm = 0; 01532 motor[TIRE_FL].dir = FREE; 01533 motor[TIRE_BR].pwm = 0; 01534 motor[TIRE_BR].dir = FREE; 01535 motor[TIRE_BL].pwm = 40; 01536 motor[TIRE_BL].dir = BACK; 01537 if(g[0]==0) { 01538 mode=10; 01539 count=100000; 01540 } 01541 } else if(mode==10) { //縦ライントレース 01542 01543 if(counts==false&&g[2]==0) { 01544 cros++; 01545 counts=true; 01546 } 01547 if(counts==true&&g[2]==99) { 01548 counts=false; 01549 } 01550 01551 if(cros==Twsh) { 01552 mode=11; 01553 count=0; 01554 cros=0; 01555 }//ゆっくりモードに入れなかった時の保険 01556 switch(g[1]) { 01557 case 98: 01558 motor[TIRE_FR].pwm = normal; 01559 motor[TIRE_FR].dir = FOR; 01560 motor[TIRE_FL].pwm = normal; 01561 motor[TIRE_FL].dir = BACK; 01562 motor[TIRE_BR].pwm = normal; 01563 motor[TIRE_BR].dir = FOR; 01564 motor[TIRE_BL].pwm = normal; 01565 motor[TIRE_BL].dir = BACK; 01566 mtc=true; 01567 if(count>80000) { 01568 cross++; 01569 count=0; 01570 } 01571 if(cross==Twsh) { 01572 mode=11; 01573 count=0; 01574 cross=0; 01575 } 01576 break; 01577 case 0: 01578 motor[TIRE_FR].pwm = fast; 01579 motor[TIRE_FR].dir = FOR; 01580 motor[TIRE_FL].pwm = fast; 01581 motor[TIRE_FL].dir = BACK; 01582 motor[TIRE_BR].pwm = fast; 01583 motor[TIRE_BR].dir = FOR; 01584 motor[TIRE_BL].pwm = fast; 01585 motor[TIRE_BL].dir = BACK; 01586 mtc=true; 01587 break; 01588 case 255: 01589 motor[TIRE_FR].pwm = fast; 01590 motor[TIRE_FR].dir = FOR; 01591 motor[TIRE_FL].pwm = slow; 01592 motor[TIRE_FL].dir = BACK; 01593 motor[TIRE_BR].pwm = slow; 01594 motor[TIRE_BR].dir = FOR; 01595 motor[TIRE_BL].pwm = fast; 01596 motor[TIRE_BL].dir = BACK; 01597 mtc=true; 01598 break; 01599 case 253: 01600 motor[TIRE_FR].pwm = slow; 01601 motor[TIRE_FR].dir = FOR; 01602 motor[TIRE_FL].pwm = 0; 01603 motor[TIRE_FL].dir = FREE; 01604 motor[TIRE_BR].pwm = 0; 01605 motor[TIRE_BR].dir = FREE; 01606 motor[TIRE_BL].pwm = slow; 01607 motor[TIRE_BL].dir = BACK; 01608 mtc=true; 01609 break; 01610 case 254: 01611 motor[TIRE_FR].pwm = normal; 01612 motor[TIRE_FR].dir = FOR; 01613 motor[TIRE_FL].pwm = 0; 01614 motor[TIRE_FL].dir = FREE; 01615 motor[TIRE_BR].pwm = 0; 01616 motor[TIRE_BR].dir = FREE; 01617 motor[TIRE_BL].pwm = normal; 01618 motor[TIRE_BL].dir = BACK; 01619 mtc=true; 01620 break; 01621 case 1: 01622 motor[TIRE_FR].pwm = slow; 01623 motor[TIRE_FR].dir = FOR; 01624 motor[TIRE_FL].pwm = fast; 01625 motor[TIRE_FL].dir = BACK; 01626 motor[TIRE_BR].pwm = fast; 01627 motor[TIRE_BR].dir = FOR; 01628 motor[TIRE_BL].pwm = slow; 01629 motor[TIRE_BL].dir = BACK; 01630 mtc=true; 01631 break; 01632 case 3: 01633 motor[TIRE_FR].pwm = 0; 01634 motor[TIRE_FR].dir = FREE; 01635 motor[TIRE_FL].pwm = slow; 01636 motor[TIRE_FL].dir = BACK; 01637 motor[TIRE_BR].pwm = slow; 01638 motor[TIRE_BR].dir = FOR; 01639 motor[TIRE_BL].pwm = 0; 01640 motor[TIRE_BL].dir = FREE; 01641 mtc=true; 01642 break; 01643 case 2: 01644 motor[TIRE_FR].pwm = 0; 01645 motor[TIRE_FR].dir = FREE; 01646 motor[TIRE_FL].pwm = normal; 01647 motor[TIRE_FL].dir = BACK; 01648 motor[TIRE_BR].pwm = normal; 01649 motor[TIRE_BR].dir = FOR; 01650 motor[TIRE_BL].pwm = 0; 01651 motor[TIRE_BL].dir = FREE; 01652 mtc=true; 01653 break; 01654 } 01655 if(g[0]!=98&&mtc==true) { 01656 switch(g[0]) { 01657 case 0: 01658 mtc=false; 01659 break; 01660 case 255: 01661 motor[TIRE_FL].pwm += 5; 01662 mtc=false; 01663 break; 01664 case 253: 01665 motor[TIRE_FL].pwm += 10; 01666 mtc=false; 01667 break; 01668 case 254: 01669 motor[TIRE_FL].pwm += 20; 01670 motor[TIRE_FR].pwm = 0; 01671 mtc=false; 01672 break; 01673 case 1: 01674 motor[TIRE_FR].pwm += 5; 01675 mtc=false; 01676 break; 01677 case 3: 01678 motor[TIRE_FR].pwm += 10; 01679 mtc=false; 01680 break; 01681 case 2: 01682 motor[TIRE_FR].pwm += 20; 01683 motor[TIRE_FL].pwm = 0; 01684 mtc=false; 01685 break; 01686 } 01687 } 01688 } else if(mode==11) { 01689 motor[TIRE_FR].pwm = normal; 01690 motor[TIRE_FR].dir = FOR; 01691 motor[TIRE_FL].pwm = 0; 01692 motor[TIRE_FL].dir = FREE; 01693 motor[TIRE_BR].pwm = 0; 01694 motor[TIRE_BR].dir = FREE; 01695 motor[TIRE_BL].pwm = normal; 01696 motor[TIRE_BL].dir = BACK; 01697 if(g[2]==0) { 01698 count=0; 01699 cross=0; 01700 mode=20; 01701 fast=40; 01702 normal=30; 01703 slow=20; 01704 } 01705 } else if(mode==20) { 01706 01707 if(Twsh==2) { 01708 solenoid.solenoid2 = SOLENOID_ON; 01709 } 01710 01711 if(counts==false&&g[1]==0) { 01712 cros++; 01713 counts=true; 01714 } 01715 if(counts==true&&g[1]==99) { 01716 counts=false; 01717 } 01718 01719 if(cros==2) { 01720 //mode=21; 01721 //count=0; 01722 //cros=0; 01723 }//ゆっくりモードに入れなかった時の保険 01724 01725 switch(g[2]) { 01726 case 98: 01727 motor[TIRE_FR].pwm = normal; 01728 motor[TIRE_FR].dir = FOR; 01729 motor[TIRE_FL].pwm = normal; 01730 motor[TIRE_FL].dir = FOR; 01731 motor[TIRE_BR].pwm = normal; 01732 motor[TIRE_BR].dir = BACK; 01733 motor[TIRE_BL].pwm = normal; 01734 motor[TIRE_BL].dir = BACK; 01735 if(count>80000) { 01736 cross++; 01737 count=0; 01738 } 01739 mtc2=true; 01740 if(cross==2) { 01741 mode=21; 01742 count=0; 01743 cross=0; 01744 } 01745 break; 01746 case 0: 01747 motor[TIRE_FR].pwm = fast; 01748 motor[TIRE_FR].dir = FOR; 01749 motor[TIRE_FL].pwm = fast; 01750 motor[TIRE_FL].dir = FOR; 01751 motor[TIRE_BR].pwm = fast; 01752 motor[TIRE_BR].dir = BACK; 01753 motor[TIRE_BL].pwm = fast; 01754 motor[TIRE_BL].dir = BACK; 01755 mtc2=true; 01756 break; 01757 case 255: 01758 motor[TIRE_FR].pwm = normal; 01759 motor[TIRE_FR].dir = FOR; 01760 motor[TIRE_FL].pwm = fast; 01761 motor[TIRE_FL].dir = FOR; 01762 motor[TIRE_BR].pwm = fast; 01763 motor[TIRE_BR].dir = BACK; 01764 motor[TIRE_BL].pwm = normal; 01765 motor[TIRE_BL].dir = BACK; 01766 mtc2=true; 01767 break; 01768 case 253: 01769 motor[TIRE_FR].pwm = 0; 01770 motor[TIRE_FR].dir = FREE; 01771 motor[TIRE_FL].pwm = slow; 01772 motor[TIRE_FL].dir = FOR; 01773 motor[TIRE_BR].pwm = slow; 01774 motor[TIRE_BR].dir = BACK; 01775 motor[TIRE_BL].pwm = 0; 01776 motor[TIRE_BL].dir = FREE; 01777 mtc2=true; 01778 break; 01779 case 254: 01780 motor[TIRE_FR].pwm = 0; 01781 motor[TIRE_FR].dir = FREE; 01782 motor[TIRE_FL].pwm = normal; 01783 motor[TIRE_FL].dir = FOR; 01784 motor[TIRE_BR].pwm = normal; 01785 motor[TIRE_BR].dir = BACK; 01786 motor[TIRE_BL].pwm = 0; 01787 motor[TIRE_BL].dir = FREE; 01788 mtc2=true; 01789 break; 01790 case 1: 01791 motor[TIRE_FR].pwm = fast; 01792 motor[TIRE_FR].dir = FOR; 01793 motor[TIRE_FL].pwm = slow; 01794 motor[TIRE_FL].dir = FOR; 01795 motor[TIRE_BR].pwm = slow; 01796 motor[TIRE_BR].dir = BACK; 01797 motor[TIRE_BL].pwm = fast; 01798 motor[TIRE_BL].dir = BACK; 01799 mtc2=true; 01800 break; 01801 case 3: 01802 motor[TIRE_FR].pwm = slow; 01803 motor[TIRE_FR].dir = FOR; 01804 motor[TIRE_FL].pwm = 0; 01805 motor[TIRE_FL].dir = FREE; 01806 motor[TIRE_BR].pwm = 0; 01807 motor[TIRE_BR].dir = FREE; 01808 motor[TIRE_BL].pwm = slow; 01809 motor[TIRE_BL].dir = BACK; 01810 mtc2=true; 01811 break; 01812 case 2: 01813 motor[TIRE_FR].pwm = normal; 01814 motor[TIRE_FR].dir = FOR; 01815 motor[TIRE_FL].pwm = 0; 01816 motor[TIRE_FL].dir = FREE; 01817 motor[TIRE_BR].pwm = 0; 01818 motor[TIRE_BR].dir = FREE; 01819 motor[TIRE_BL].pwm = normal; 01820 motor[TIRE_BL].dir = BACK; 01821 mtc2=true; 01822 break; 01823 } 01824 if(g[2]!=98&&mtc2==true) { 01825 switch(g[3]) { 01826 case 0: 01827 mtc2=false; 01828 break; 01829 case 255: 01830 motor[TIRE_BL].pwm += 5; 01831 mtc2=false; 01832 break; 01833 case 253: 01834 motor[TIRE_BL].pwm += 10; 01835 mtc2=false; 01836 break; 01837 case 254: 01838 motor[TIRE_BL].pwm += 20; 01839 motor[TIRE_FL].pwm = 0; 01840 mtc2=false; 01841 break; 01842 case 1: 01843 motor[TIRE_FL].pwm += 5; 01844 mtc2=false; 01845 break; 01846 case 3: 01847 motor[TIRE_FL].pwm += 10; 01848 mtc2=false; 01849 break; 01850 case 2: 01851 motor[TIRE_FL].pwm += 20; 01852 motor[TIRE_BL].pwm = 0; 01853 mtc2=false; 01854 break; 01855 } 01856 } 01857 } else if(mode==21) { 01858 motor[TIRE_FR].pwm = normal; 01859 motor[TIRE_FR].dir = FOR; 01860 motor[TIRE_FL].pwm = 0; 01861 motor[TIRE_FL].dir = FREE; 01862 motor[TIRE_BR].pwm = 0; 01863 motor[TIRE_BR].dir = FREE; 01864 motor[TIRE_BL].pwm = normal; 01865 motor[TIRE_BL].dir = BACK; 01866 if(g[1]==0) { 01867 mode=30; 01868 cros=0; 01869 zyouge=true; 01870 fast=80; 01871 normal=60; 01872 slow=40; 01873 } 01874 } else if(mode==30) { 01875 motor[TIRE_FR].pwm = 60; 01876 motor[TIRE_FR].dir = FOR; 01877 motor[TIRE_FL].pwm = 60; 01878 motor[TIRE_FL].dir = BACK; 01879 motor[TIRE_BR].pwm = 60; 01880 motor[TIRE_BR].dir = FOR; 01881 motor[TIRE_BL].pwm = 50; 01882 motor[TIRE_BL].dir = BACK; 01883 if(g[1]==98) { 01884 mode=31; 01885 } 01886 if(g[3]==0) { 01887 mode=32; 01888 count=0; 01889 cros=0; 01890 }//ゆっくりモードに入れなかった時の保険 01891 /* 01892 switch(g[1]) { 01893 case 98: 01894 motor[TIRE_FR].pwm = 30; 01895 motor[TIRE_FR].dir = FOR; 01896 motor[TIRE_FL].pwm = 30; 01897 motor[TIRE_FL].dir = BACK; 01898 motor[TIRE_BR].pwm = 30; 01899 motor[TIRE_BR].dir = FOR; 01900 motor[TIRE_BL].pwm = 30; 01901 motor[TIRE_BL].dir = BACK; 01902 if(count>100000) { 01903 cross++; 01904 count=0; 01905 } 01906 mtc=true; 01907 if(cross==1) { 01908 if(Twsh==2) { 01909 solenoid.solenoid2 = SOLENOID_OFF; 01910 } 01911 mode=31; 01912 count=0; 01913 } 01914 break; 01915 case 0: 01916 motor[TIRE_FR].pwm = 60; 01917 motor[TIRE_FR].dir = FOR; 01918 motor[TIRE_FL].pwm = 60; 01919 motor[TIRE_FL].dir = BACK; 01920 motor[TIRE_BR].pwm = 60; 01921 motor[TIRE_BR].dir = FOR; 01922 motor[TIRE_BL].pwm = 60; 01923 motor[TIRE_BL].dir = BACK; 01924 mtc=true; 01925 break; 01926 case 255: 01927 motor[TIRE_FR].pwm = 60; 01928 motor[TIRE_FR].dir = FOR; 01929 motor[TIRE_FL].pwm = 20; 01930 motor[TIRE_FL].dir = BACK; 01931 motor[TIRE_BR].pwm = 20; 01932 motor[TIRE_BR].dir = FOR; 01933 motor[TIRE_BL].pwm = 60; 01934 motor[TIRE_BL].dir = BACK; 01935 mtc=true; 01936 break; 01937 case 253: 01938 motor[TIRE_FR].pwm = 20; 01939 motor[TIRE_FR].dir = FOR; 01940 motor[TIRE_FL].pwm = 0; 01941 motor[TIRE_FL].dir = BACK; 01942 motor[TIRE_BR].pwm = 0; 01943 motor[TIRE_BR].dir = FOR; 01944 motor[TIRE_BL].pwm = 20; 01945 motor[TIRE_BL].dir = BACK; 01946 mtc=true; 01947 break; 01948 case 254: 01949 motor[TIRE_FR].pwm = 30; 01950 motor[TIRE_FR].dir = FOR; 01951 motor[TIRE_FL].pwm = 0; 01952 motor[TIRE_FL].dir = FREE; 01953 motor[TIRE_BR].pwm = 0; 01954 motor[TIRE_BR].dir = FREE; 01955 motor[TIRE_BL].pwm = 30; 01956 motor[TIRE_BL].dir = BACK; 01957 mtc=true; 01958 break; 01959 case 1: 01960 motor[TIRE_FR].pwm = 20; 01961 motor[TIRE_FR].dir = FOR; 01962 motor[TIRE_FL].pwm = 60; 01963 motor[TIRE_FL].dir = BACK; 01964 motor[TIRE_BR].pwm = 60; 01965 motor[TIRE_BR].dir = FOR; 01966 motor[TIRE_BL].pwm = 20; 01967 motor[TIRE_BL].dir = BACK; 01968 mtc=true; 01969 break; 01970 case 3: 01971 motor[TIRE_FR].pwm = 0; 01972 motor[TIRE_FR].dir = FREE; 01973 motor[TIRE_FL].pwm = 20; 01974 motor[TIRE_FL].dir = BACK; 01975 motor[TIRE_BR].pwm = 20; 01976 motor[TIRE_BR].dir = FOR; 01977 motor[TIRE_BL].pwm = 0; 01978 motor[TIRE_BL].dir = FREE; 01979 mtc=true; 01980 break; 01981 case 2: 01982 motor[TIRE_FR].pwm = 0; 01983 motor[TIRE_FR].dir = FREE; 01984 motor[TIRE_FL].pwm = 30; 01985 motor[TIRE_FL].dir = BACK; 01986 motor[TIRE_BR].pwm = 30; 01987 motor[TIRE_BR].dir = FOR; 01988 motor[TIRE_BL].pwm = 0; 01989 motor[TIRE_BL].dir = FREE; 01990 mtc=true; 01991 break; 01992 } 01993 if(g[1]!=98&&mtc==true) { 01994 switch(g[0]) { 01995 case 0: 01996 mtc=false; 01997 break; 01998 case 255: 01999 motor[TIRE_FL].pwm += 5; 02000 mtc=false; 02001 break; 02002 case 253: 02003 motor[TIRE_FL].pwm += 10; 02004 mtc=false; 02005 break; 02006 case 254: 02007 motor[TIRE_FL].pwm += 20; 02008 motor[TIRE_FR].pwm = 0; 02009 mtc=false; 02010 break; 02011 case 1: 02012 motor[TIRE_FR].pwm += 5; 02013 mtc=false; 02014 break; 02015 case 3: 02016 motor[TIRE_FR].pwm += 10; 02017 mtc=false; 02018 break; 02019 case 2: 02020 motor[TIRE_FR].pwm += 20; 02021 motor[TIRE_FL].pwm = 0; 02022 mtc=false; 02023 break; 02024 } 02025 } 02026 */ 02027 } else if(mode==31) { 02028 motor[TIRE_FR].pwm = 15; 02029 motor[TIRE_FR].dir = FOR; 02030 motor[TIRE_FL].pwm = 15; 02031 motor[TIRE_FL].dir = BACK; 02032 motor[TIRE_BR].pwm = 15; 02033 motor[TIRE_BR].dir = FOR; 02034 motor[TIRE_BL].pwm = 15; 02035 motor[TIRE_BL].dir = BACK; 02036 if(g[3]==0) { 02037 count=0; 02038 mode=32; 02039 } 02040 } else if(mode==32) { 02041 motor[TIRE_FR].pwm = 30; 02042 motor[TIRE_FR].dir = BACK; 02043 motor[TIRE_FL].pwm = 30; 02044 motor[TIRE_FL].dir = FOR; 02045 motor[TIRE_BR].pwm = 30; 02046 motor[TIRE_BR].dir = BACK; 02047 motor[TIRE_BL].pwm = 30; 02048 motor[TIRE_BL].dir = FOR; 02049 if(g[3]==0) { 02050 count=0; 02051 if(Twsh==2) { 02052 solenoid.solenoid2 = SOLENOID_OFF; 02053 } 02054 mode=40; 02055 } 02056 } else if(mode==40) { 02057 02058 if(LIF) { 02059 mode=42; 02060 } 02061 02062 if(counts==false&&g[0]==0) { 02063 cros++; 02064 counts=true; 02065 } 02066 if(counts==true&&g[0]==99) { 02067 counts=false; 02068 } 02069 02070 if(cros==1) { 02071 // mode=41; 02072 //count=0; 02073 //cros=0; 02074 }//ゆっくりモードに入れなかった時の保険 02075 02076 switch(g[3]) { 02077 case 98: 02078 motor[TIRE_FR].pwm = normal; 02079 motor[TIRE_FR].dir = BACK; 02080 motor[TIRE_FL].pwm = normal; 02081 motor[TIRE_FL].dir = BACK; 02082 motor[TIRE_BR].pwm = normal; 02083 motor[TIRE_BR].dir = FOR; 02084 motor[TIRE_BL].pwm = normal; 02085 motor[TIRE_BL].dir = FOR; 02086 if(count>20000) { 02087 cross++; 02088 count=0; 02089 } 02090 mtc2=true; 02091 if(cross==2) { 02092 mode=41; 02093 count=0; 02094 cross=0; 02095 } 02096 break; 02097 case 0: 02098 motor[TIRE_FR].pwm = fast; 02099 motor[TIRE_FR].dir = BACK; 02100 motor[TIRE_FL].pwm = fast; 02101 motor[TIRE_FL].dir = BACK; 02102 motor[TIRE_BR].pwm = fast; 02103 motor[TIRE_BR].dir = FOR; 02104 motor[TIRE_BL].pwm = fast; 02105 motor[TIRE_BL].dir = FOR; 02106 mtc2=true; 02107 break; 02108 case 255: 02109 motor[TIRE_FR].pwm = slow; 02110 motor[TIRE_FR].dir = BACK; 02111 motor[TIRE_FL].pwm = fast; 02112 motor[TIRE_FL].dir = BACK; 02113 motor[TIRE_BR].pwm = fast; 02114 motor[TIRE_BR].dir = FOR; 02115 motor[TIRE_BL].pwm = slow; 02116 motor[TIRE_BL].dir = FOR; 02117 mtc2=true; 02118 break; 02119 case 253: 02120 motor[TIRE_FR].pwm = 0; 02121 motor[TIRE_FR].dir = FREE; 02122 motor[TIRE_FL].pwm = slow; 02123 motor[TIRE_FL].dir = BACK; 02124 motor[TIRE_BR].pwm = slow; 02125 motor[TIRE_BR].dir = FOR; 02126 motor[TIRE_BL].pwm = 0; 02127 motor[TIRE_BL].dir = FREE; 02128 mtc2=true; 02129 break; 02130 case 254: 02131 motor[TIRE_FR].pwm = 0; 02132 motor[TIRE_FR].dir = FREE; 02133 motor[TIRE_FL].pwm = normal; 02134 motor[TIRE_FL].dir = BACK; 02135 motor[TIRE_BR].pwm = normal; 02136 motor[TIRE_BR].dir = FOR; 02137 motor[TIRE_BL].pwm = 0; 02138 motor[TIRE_BL].dir = FREE; 02139 mtc2=true; 02140 break; 02141 case 1: 02142 motor[TIRE_FR].pwm = fast; 02143 motor[TIRE_FR].dir = BACK; 02144 motor[TIRE_FL].pwm = slow; 02145 motor[TIRE_FL].dir = BACK; 02146 motor[TIRE_BR].pwm = slow; 02147 motor[TIRE_BR].dir = FOR; 02148 motor[TIRE_BL].pwm = fast; 02149 motor[TIRE_BL].dir = FOR; 02150 mtc2=true; 02151 break; 02152 case 3: 02153 motor[TIRE_FR].pwm = slow; 02154 motor[TIRE_FR].dir = BACK; 02155 motor[TIRE_FL].pwm = 0; 02156 motor[TIRE_FL].dir = FREE; 02157 motor[TIRE_BR].pwm = 0; 02158 motor[TIRE_BR].dir = FREE; 02159 motor[TIRE_BL].pwm = slow; 02160 motor[TIRE_BL].dir = FOR; 02161 mtc2=true; 02162 break; 02163 case 2: 02164 motor[TIRE_FR].pwm = normal; 02165 motor[TIRE_FR].dir = BACK; 02166 motor[TIRE_FL].pwm = 0; 02167 motor[TIRE_FL].dir = FREE; 02168 motor[TIRE_BR].pwm = 0; 02169 motor[TIRE_BR].dir = FREE; 02170 motor[TIRE_BL].pwm = normal; 02171 motor[TIRE_BL].dir = FOR; 02172 mtc2=true; 02173 break; 02174 } 02175 if(g[2]!=98&&mtc2==true) { 02176 switch(g[2]) { 02177 case 0: 02178 mtc2=false; 02179 break; 02180 case 255: 02181 motor[TIRE_FR].pwm += 5; 02182 mtc2=false; 02183 break; 02184 case 253: 02185 motor[TIRE_FR].pwm += 10; 02186 mtc2=false; 02187 break; 02188 case 254: 02189 motor[TIRE_FR].pwm += 20; 02190 motor[TIRE_BR].pwm = 0; 02191 mtc2=false; 02192 break; 02193 case 1: 02194 motor[TIRE_BR].pwm += 5; 02195 mtc2=false; 02196 break; 02197 case 3: 02198 motor[TIRE_BR].pwm += 10; 02199 mtc2=false; 02200 break; 02201 case 2: 02202 motor[TIRE_BR].pwm += 20; 02203 motor[TIRE_FR].pwm = 0; 02204 mtc2=false; 02205 break; 02206 } 02207 } 02208 } else if(mode==41) { 02209 motor[TIRE_FR].pwm = 30; 02210 motor[TIRE_FR].dir = BACK; 02211 motor[TIRE_FL].pwm = 0; 02212 motor[TIRE_FL].dir = FREE; 02213 motor[TIRE_BR].pwm = 0; 02214 motor[TIRE_BR].dir = FREE; 02215 motor[TIRE_BL].pwm = 30; 02216 motor[TIRE_BL].dir = FOR; 02217 if(g[1]==0) { 02218 count=0; 02219 mode=70; 02220 normal=60; 02221 fast=80; 02222 slow=40; 02223 } 02224 } else if(mode==70) 02225 switch(g[0]) { 02226 case 99: 02227 if(count>=100000) { 02228 cross++; 02229 mode=100; 02230 count=0; 02231 } 02232 if(cross==3) { 02233 cross=0; 02234 mode=100; 02235 } 02236 break; 02237 case 98: 02238 motor[TIRE_FR].pwm = normal; 02239 motor[TIRE_FR].dir = BACK; 02240 motor[TIRE_FL].pwm = normal; 02241 motor[TIRE_FL].dir = FOR; 02242 motor[TIRE_BR].pwm = normal; 02243 motor[TIRE_BR].dir = BACK; 02244 motor[TIRE_BL].pwm = normal; 02245 motor[TIRE_BL].dir = FOR; 02246 break; 02247 case 0: 02248 motor[TIRE_FR].pwm = fast; 02249 motor[TIRE_FR].dir = BACK; 02250 motor[TIRE_FL].pwm = fast; 02251 motor[TIRE_FL].dir = FOR; 02252 motor[TIRE_BR].pwm = fast; 02253 motor[TIRE_BR].dir = BACK; 02254 motor[TIRE_BL].pwm = fast; 02255 motor[TIRE_BL].dir = FOR; 02256 mtc=true; 02257 break; 02258 case 255: 02259 motor[TIRE_FR].pwm = fast; 02260 motor[TIRE_FR].dir = BACK; 02261 motor[TIRE_FL].pwm = slow; 02262 motor[TIRE_FL].dir = FOR; 02263 motor[TIRE_BR].pwm = slow; 02264 motor[TIRE_BR].dir = BACK; 02265 motor[TIRE_BL].pwm = fast; 02266 motor[TIRE_BL].dir = FOR; 02267 mtc=true; 02268 break; 02269 case 253: 02270 motor[TIRE_FR].pwm = slow; 02271 motor[TIRE_FR].dir = BACK; 02272 motor[TIRE_FL].pwm = 0; 02273 motor[TIRE_FL].dir = FREE; 02274 motor[TIRE_BR].pwm = 0; 02275 motor[TIRE_BR].dir = FREE; 02276 motor[TIRE_BL].pwm = slow; 02277 motor[TIRE_BL].dir = FOR; 02278 mtc=true; 02279 break; 02280 case 254: 02281 motor[TIRE_FR].pwm = normal; 02282 motor[TIRE_FR].dir = BACK; 02283 motor[TIRE_FL].pwm = 0; 02284 motor[TIRE_FL].dir = FREE; 02285 motor[TIRE_BR].pwm = 0; 02286 motor[TIRE_BR].dir = FREE; 02287 motor[TIRE_BL].pwm = normal; 02288 motor[TIRE_BL].dir = FOR; 02289 mtc=true; 02290 break; 02291 case 1: 02292 motor[TIRE_FR].pwm = slow; 02293 motor[TIRE_FR].dir = BACK; 02294 motor[TIRE_FL].pwm = fast; 02295 motor[TIRE_FL].dir = FOR; 02296 motor[TIRE_BR].pwm = fast; 02297 motor[TIRE_BR].dir = BACK; 02298 motor[TIRE_BL].pwm = slow; 02299 motor[TIRE_BL].dir = FOR; 02300 mtc=true; 02301 break; 02302 case 3: 02303 motor[TIRE_FR].pwm = 0; 02304 motor[TIRE_FR].dir = FREE; 02305 motor[TIRE_FL].pwm = slow; 02306 motor[TIRE_FL].dir = FOR; 02307 motor[TIRE_BR].pwm = slow; 02308 motor[TIRE_BR].dir = BACK; 02309 motor[TIRE_BL].pwm = 0; 02310 motor[TIRE_BL].dir = FREE; 02311 mtc=true; 02312 break; 02313 case 2: 02314 motor[TIRE_FR].pwm = 0; 02315 motor[TIRE_FR].dir = FREE; 02316 motor[TIRE_FL].pwm = normal; 02317 motor[TIRE_FL].dir = FOR; 02318 motor[TIRE_BR].pwm = normal; 02319 motor[TIRE_BR].dir = BACK; 02320 motor[TIRE_BL].pwm = 0; 02321 motor[TIRE_BL].dir = FREE; 02322 mtc=true; 02323 break; 02324 } 02325 if(g[1]!=98&&mtc==true) { 02326 switch(g[1]) { 02327 case 0: 02328 mtc=false; 02329 break; 02330 case 255: 02331 motor[TIRE_BR].pwm += 5; 02332 mtc=false; 02333 break; 02334 case 253: 02335 motor[TIRE_BR].pwm += 10; 02336 mtc=false; 02337 break; 02338 case 254: 02339 motor[TIRE_BR].pwm += 20; 02340 motor[TIRE_BL].pwm = 0; 02341 mtc=false; 02342 break; 02343 case 1: 02344 motor[TIRE_BL].pwm += 5; 02345 mtc=false; 02346 break; 02347 case 3: 02348 motor[TIRE_BL].pwm += 10; 02349 mtc=false; 02350 break; 02351 case 2: 02352 motor[TIRE_BL].pwm += 20; 02353 motor[TIRE_BR].pwm = 0; 02354 mtc=false; 02355 break; 02356 } 02357 } else if(mode==100) { 02358 motor[TIRE_FR].pwm = 20; 02359 motor[TIRE_FR].dir = BACK; 02360 motor[TIRE_FL].pwm = 20; 02361 motor[TIRE_FL].dir = FOR; 02362 motor[TIRE_BR].pwm = 23; 02363 motor[TIRE_BR].dir = BACK; 02364 motor[TIRE_BL].pwm = 20; 02365 motor[TIRE_BL].dir = FOR; 02366 if(StertSW) { 02367 SW_flag = true; 02368 } 02369 if(SW_flag ==true) { 02370 motor[TIRE_FR].pwm = 100; 02371 motor[TIRE_FR].dir = BRAKE; 02372 motor[TIRE_FL].pwm = 100; 02373 motor[TIRE_FL].dir = BRAKE; 02374 motor[TIRE_BR].pwm = 100; 02375 motor[TIRE_BR].dir = BRAKE; 02376 motor[TIRE_BL].pwm = 100; 02377 motor[TIRE_BL].dir = BRAKE; 02378 fast=60; 02379 normal=40; 02380 slow=20; 02381 mode =1; 02382 cross=0;//十字数える用 02383 cros=0; 02384 count=100000;//wait代替え 02385 UP_flag = false; 02386 SW_flag = false; 02387 Air_flag = false; 02388 zyouge=false; 02389 zyougedo=false; 02390 dz1=true; 02391 dz1i=false; 02392 dz2=true; 02393 dz2i=false; 02394 dz3=true; 02395 dz3i=false; 02396 dz4=true; 02397 dz4i=false; 02398 through=false; 02399 counts=false; 02400 mtc=false; 02401 mtc2 = false; 02402 current = 0; 02403 if(StertSW==false) { 02404 SW_flag = false; 02405 } 02406 } 02407 } 02408 02409 02410 } 02411 #endif 02412 02413 #if USE_PROCESS_NUM>5 02414 static void Process5() 02415 { 02416 LedMode(5); 02417 count++; 02418 if(mode==1) {//スタートゾーンから白線検知 02419 fast=50; 02420 normal=30; 02421 slow=20; 02422 motor[TIRE_FR].pwm = 35; 02423 motor[TIRE_FR].dir = FOR; 02424 motor[TIRE_FL].pwm = 35; 02425 motor[TIRE_FL].dir = BACK; 02426 motor[TIRE_BR].pwm = 35; 02427 motor[TIRE_BR].dir = FOR; 02428 motor[TIRE_BL].pwm = 32; 02429 motor[TIRE_BL].dir = BACK; 02430 if(g[2]==0) { 02431 count=100000; 02432 cross=0; 02433 mode=3; 02434 } 02435 } else if(mode==3) { //横ライントレースから縦ライントレースへ 02436 motor[TIRE_FR].pwm = 27; 02437 motor[TIRE_FR].dir = FOR; 02438 motor[TIRE_FL].pwm = 0; 02439 motor[TIRE_FL].dir = FREE; 02440 motor[TIRE_BR].pwm = 0; 02441 motor[TIRE_BR].dir = FREE; 02442 motor[TIRE_BL].pwm = 25; 02443 motor[TIRE_BL].dir = BACK; 02444 if(g[0]==0) { 02445 mode=10; 02446 count=100000; 02447 } 02448 } else if(mode==10) { //縦ライントレース 02449 02450 if(counts==false&&g[2]==0) { 02451 cros++; 02452 counts=true; 02453 } 02454 if(counts==true&&g[2]==99) { 02455 counts=false; 02456 } 02457 02458 if(cros==Twsh) { 02459 mode=11; 02460 count=0; 02461 cros=0; 02462 }//ゆっくりモードに入れなかった時の保険 02463 switch(g[1]) { 02464 case 98: 02465 motor[TIRE_FR].pwm = normal; 02466 motor[TIRE_FR].dir = FOR; 02467 motor[TIRE_FL].pwm = normal; 02468 motor[TIRE_FL].dir = BACK; 02469 motor[TIRE_BR].pwm = normal; 02470 motor[TIRE_BR].dir = FOR; 02471 motor[TIRE_BL].pwm = normal; 02472 motor[TIRE_BL].dir = BACK; 02473 mtc=true; 02474 if(count>80000) { 02475 cross++; 02476 count=0; 02477 } 02478 if(cross==Twsh) { 02479 mode=11; 02480 count=0; 02481 cross=0; 02482 } 02483 break; 02484 case 0: 02485 motor[TIRE_FR].pwm = fast; 02486 motor[TIRE_FR].dir = FOR; 02487 motor[TIRE_FL].pwm = fast; 02488 motor[TIRE_FL].dir = BACK; 02489 motor[TIRE_BR].pwm = fast; 02490 motor[TIRE_BR].dir = FOR; 02491 motor[TIRE_BL].pwm = fast; 02492 motor[TIRE_BL].dir = BACK; 02493 mtc=true; 02494 break; 02495 case 255: 02496 motor[TIRE_FR].pwm = fast; 02497 motor[TIRE_FR].dir = FOR; 02498 motor[TIRE_FL].pwm = slow; 02499 motor[TIRE_FL].dir = BACK; 02500 motor[TIRE_BR].pwm = slow; 02501 motor[TIRE_BR].dir = FOR; 02502 motor[TIRE_BL].pwm = fast; 02503 motor[TIRE_BL].dir = BACK; 02504 mtc=true; 02505 break; 02506 case 253: 02507 motor[TIRE_FR].pwm = slow; 02508 motor[TIRE_FR].dir = FOR; 02509 motor[TIRE_FL].pwm = 0; 02510 motor[TIRE_FL].dir = FREE; 02511 motor[TIRE_BR].pwm = 0; 02512 motor[TIRE_BR].dir = FREE; 02513 motor[TIRE_BL].pwm = slow; 02514 motor[TIRE_BL].dir = BACK; 02515 mtc=true; 02516 break; 02517 case 254: 02518 motor[TIRE_FR].pwm = normal; 02519 motor[TIRE_FR].dir = FOR; 02520 motor[TIRE_FL].pwm = 0; 02521 motor[TIRE_FL].dir = FREE; 02522 motor[TIRE_BR].pwm = 0; 02523 motor[TIRE_BR].dir = FREE; 02524 motor[TIRE_BL].pwm = normal; 02525 motor[TIRE_BL].dir = BACK; 02526 mtc=true; 02527 break; 02528 case 1: 02529 motor[TIRE_FR].pwm = slow; 02530 motor[TIRE_FR].dir = FOR; 02531 motor[TIRE_FL].pwm = fast; 02532 motor[TIRE_FL].dir = BACK; 02533 motor[TIRE_BR].pwm = fast; 02534 motor[TIRE_BR].dir = FOR; 02535 motor[TIRE_BL].pwm = slow; 02536 motor[TIRE_BL].dir = BACK; 02537 mtc=true; 02538 break; 02539 case 3: 02540 motor[TIRE_FR].pwm = 0; 02541 motor[TIRE_FR].dir = FREE; 02542 motor[TIRE_FL].pwm = slow; 02543 motor[TIRE_FL].dir = BACK; 02544 motor[TIRE_BR].pwm = slow; 02545 motor[TIRE_BR].dir = FOR; 02546 motor[TIRE_BL].pwm = 0; 02547 motor[TIRE_BL].dir = FREE; 02548 mtc=true; 02549 break; 02550 case 2: 02551 motor[TIRE_FR].pwm = 0; 02552 motor[TIRE_FR].dir = FREE; 02553 motor[TIRE_FL].pwm = normal; 02554 motor[TIRE_FL].dir = BACK; 02555 motor[TIRE_BR].pwm = normal; 02556 motor[TIRE_BR].dir = FOR; 02557 motor[TIRE_BL].pwm = 0; 02558 motor[TIRE_BL].dir = FREE; 02559 mtc=true; 02560 break; 02561 } 02562 if(g[0]!=98&&mtc==true) { 02563 switch(g[0]) { 02564 case 0: 02565 mtc=false; 02566 break; 02567 case 255: 02568 motor[TIRE_FL].pwm += 5; 02569 mtc=false; 02570 break; 02571 case 253: 02572 motor[TIRE_FL].pwm += 10; 02573 mtc=false; 02574 break; 02575 case 254: 02576 motor[TIRE_FL].pwm += 20; 02577 motor[TIRE_FR].pwm = 0; 02578 mtc=false; 02579 break; 02580 case 1: 02581 motor[TIRE_FR].pwm += 5; 02582 mtc=false; 02583 break; 02584 case 3: 02585 motor[TIRE_FR].pwm += 10; 02586 mtc=false; 02587 break; 02588 case 2: 02589 motor[TIRE_FR].pwm += 20; 02590 motor[TIRE_FL].pwm = 0; 02591 mtc=false; 02592 break; 02593 } 02594 } 02595 } else if(mode==11) { 02596 motor[TIRE_FR].pwm = normal; 02597 motor[TIRE_FR].dir = FOR; 02598 motor[TIRE_FL].pwm = 0; 02599 motor[TIRE_FL].dir = FREE; 02600 motor[TIRE_BR].pwm = 0; 02601 motor[TIRE_BR].dir = FREE; 02602 motor[TIRE_BL].pwm = normal; 02603 motor[TIRE_BL].dir = BACK; 02604 if(g[2]==0) { 02605 count=0; 02606 cross=0; 02607 mode=20; 02608 } 02609 } else if(mode==20) { 02610 02611 if(Twsh==2) { 02612 solenoid.solenoid2 = SOLENOID_ON; 02613 } 02614 02615 if(counts==false&&g[1]==0) { 02616 cros++; 02617 counts=true; 02618 } 02619 if(counts==true&&g[1]==99) { 02620 counts=false; 02621 } 02622 02623 if(cros==2) { 02624 //mode=21; 02625 //count=0; 02626 //cros=0; 02627 }//ゆっくりモードに入れなかった時の保険 02628 02629 switch(g[2]) { 02630 case 98: 02631 motor[TIRE_FR].pwm = normal; 02632 motor[TIRE_FR].dir = FOR; 02633 motor[TIRE_FL].pwm = normal; 02634 motor[TIRE_FL].dir = FOR; 02635 motor[TIRE_BR].pwm = normal; 02636 motor[TIRE_BR].dir = BACK; 02637 motor[TIRE_BL].pwm = normal; 02638 motor[TIRE_BL].dir = BACK; 02639 if(count>50000) { 02640 cross++; 02641 count=0; 02642 } 02643 mtc2=true; 02644 if(cross==1) { 02645 fast=30; 02646 normal=20; 02647 slow=20; 02648 } 02649 if(cross==2) { 02650 mode=21; 02651 cross=0; 02652 count=0; 02653 } 02654 break; 02655 case 0: 02656 motor[TIRE_FR].pwm = fast; 02657 motor[TIRE_FR].dir = FOR; 02658 motor[TIRE_FL].pwm = fast; 02659 motor[TIRE_FL].dir = FOR; 02660 motor[TIRE_BR].pwm = fast; 02661 motor[TIRE_BR].dir = BACK; 02662 motor[TIRE_BL].pwm = fast; 02663 motor[TIRE_BL].dir = BACK; 02664 mtc2=true; 02665 break; 02666 case 255: 02667 motor[TIRE_FR].pwm = normal; 02668 motor[TIRE_FR].dir = FOR; 02669 motor[TIRE_FL].pwm = fast; 02670 motor[TIRE_FL].dir = FOR; 02671 motor[TIRE_BR].pwm = fast; 02672 motor[TIRE_BR].dir = BACK; 02673 motor[TIRE_BL].pwm = normal; 02674 motor[TIRE_BL].dir = BACK; 02675 mtc2=true; 02676 break; 02677 case 253: 02678 motor[TIRE_FR].pwm = 0; 02679 motor[TIRE_FR].dir = FREE; 02680 motor[TIRE_FL].pwm = slow; 02681 motor[TIRE_FL].dir = FOR; 02682 motor[TIRE_BR].pwm = slow; 02683 motor[TIRE_BR].dir = BACK; 02684 motor[TIRE_BL].pwm = 0; 02685 motor[TIRE_BL].dir = FREE; 02686 mtc2=true; 02687 break; 02688 case 254: 02689 motor[TIRE_FR].pwm = 0; 02690 motor[TIRE_FR].dir = FREE; 02691 motor[TIRE_FL].pwm = normal; 02692 motor[TIRE_FL].dir = FOR; 02693 motor[TIRE_BR].pwm = normal; 02694 motor[TIRE_BR].dir = BACK; 02695 motor[TIRE_BL].pwm = 0; 02696 motor[TIRE_BL].dir = FREE; 02697 mtc2=true; 02698 break; 02699 case 1: 02700 motor[TIRE_FR].pwm = fast; 02701 motor[TIRE_FR].dir = FOR; 02702 motor[TIRE_FL].pwm = slow; 02703 motor[TIRE_FL].dir = FOR; 02704 motor[TIRE_BR].pwm = slow; 02705 motor[TIRE_BR].dir = BACK; 02706 motor[TIRE_BL].pwm = fast; 02707 motor[TIRE_BL].dir = BACK; 02708 mtc2=true; 02709 break; 02710 case 3: 02711 motor[TIRE_FR].pwm = slow; 02712 motor[TIRE_FR].dir = FOR; 02713 motor[TIRE_FL].pwm = 0; 02714 motor[TIRE_FL].dir = FREE; 02715 motor[TIRE_BR].pwm = 0; 02716 motor[TIRE_BR].dir = FREE; 02717 motor[TIRE_BL].pwm = slow; 02718 motor[TIRE_BL].dir = BACK; 02719 mtc2=true; 02720 break; 02721 case 2: 02722 motor[TIRE_FR].pwm = normal; 02723 motor[TIRE_FR].dir = FOR; 02724 motor[TIRE_FL].pwm = 0; 02725 motor[TIRE_FL].dir = FREE; 02726 motor[TIRE_BR].pwm = 0; 02727 motor[TIRE_BR].dir = FREE; 02728 motor[TIRE_BL].pwm = normal; 02729 motor[TIRE_BL].dir = BACK; 02730 mtc2=true; 02731 break; 02732 } 02733 if(g[2]!=98&&mtc2==true) { 02734 switch(g[3]) { 02735 case 0: 02736 mtc2=false; 02737 break; 02738 case 255: 02739 motor[TIRE_BL].pwm += 5; 02740 mtc2=false; 02741 break; 02742 case 253: 02743 motor[TIRE_BL].pwm += 10; 02744 mtc2=false; 02745 break; 02746 case 254: 02747 motor[TIRE_BL].pwm += 20; 02748 motor[TIRE_FL].pwm = 0; 02749 mtc2=false; 02750 break; 02751 case 1: 02752 motor[TIRE_FL].pwm += 5; 02753 mtc2=false; 02754 break; 02755 case 3: 02756 motor[TIRE_FL].pwm += 10; 02757 mtc2=false; 02758 break; 02759 case 2: 02760 motor[TIRE_FL].pwm += 20; 02761 motor[TIRE_BL].pwm = 0; 02762 mtc2=false; 02763 break; 02764 } 02765 } 02766 } else if(mode==21) { 02767 motor[TIRE_FR].pwm = 20; 02768 motor[TIRE_FR].dir = BRAKE; 02769 motor[TIRE_FL].pwm = 20; 02770 motor[TIRE_FL].dir = BRAKE; 02771 motor[TIRE_BR].pwm = 20; 02772 motor[TIRE_BR].dir = BRAKE; 02773 motor[TIRE_BL].pwm = 20; 02774 motor[TIRE_BL].dir = BRAKE; 02775 if(count<=100000) { 02776 mode=22; 02777 } 02778 } else if(mode==22) { 02779 motor[TIRE_FR].pwm = 20; 02780 motor[TIRE_FR].dir = BACK; 02781 motor[TIRE_FL].pwm = 20; 02782 motor[TIRE_FL].dir = BACK; 02783 motor[TIRE_BR].pwm = 20; 02784 motor[TIRE_BR].dir = FOR; 02785 motor[TIRE_BL].pwm = 20; 02786 motor[TIRE_BL].dir = FOR; 02787 if(g[2]!=98) { 02788 mode=30; 02789 } 02790 } else if(mode==30) { 02791 motor[TIRE_FR].pwm = 64; 02792 motor[TIRE_FR].dir = FOR; 02793 motor[TIRE_FL].pwm = 60; 02794 motor[TIRE_FL].dir = BACK; 02795 motor[TIRE_BR].pwm = 60; 02796 motor[TIRE_BR].dir = FOR; 02797 motor[TIRE_BL].pwm = 60; 02798 motor[TIRE_BL].dir = BACK; 02799 if(g[1]==98) { 02800 mode=31; 02801 count=0; 02802 } 02803 } else if(mode==31) { 02804 motor[TIRE_FR].pwm = 10; 02805 motor[TIRE_FR].dir = FREE; 02806 motor[TIRE_FL].pwm = 10; 02807 motor[TIRE_FL].dir = FREE; 02808 motor[TIRE_BR].pwm = 10; 02809 motor[TIRE_BR].dir = FREE; 02810 motor[TIRE_BL].pwm = 10; 02811 motor[TIRE_BL].dir = FREE; 02812 if(count>=10000) { 02813 mode=32; 02814 count=0; 02815 } 02816 } else if(mode==32) { 02817 motor[TIRE_FR].pwm = 5; 02818 motor[TIRE_FR].dir = BRAKE; 02819 motor[TIRE_FL].pwm = 5; 02820 motor[TIRE_FL].dir = BRAKE; 02821 motor[TIRE_BR].pwm = 5; 02822 motor[TIRE_BR].dir = BRAKE; 02823 motor[TIRE_BL].pwm = 5; 02824 motor[TIRE_BL].dir = BRAKE; 02825 if(count>=40000) { 02826 mode=33; 02827 count=0; 02828 } 02829 } else if(mode==33) { 02830 motor[TIRE_FR].pwm = 0; 02831 motor[TIRE_FR].dir = FREE; 02832 motor[TIRE_FL].pwm = 25; 02833 motor[TIRE_FL].dir = BACK; 02834 motor[TIRE_BR].pwm = 25; 02835 motor[TIRE_BR].dir = FOR; 02836 motor[TIRE_BL].pwm = 0; 02837 motor[TIRE_BL].dir = FREE; 02838 if(g[3]==0) { 02839 count=0; 02840 if(Twsh==2) { 02841 solenoid.solenoid2 = SOLENOID_OFF; 02842 } 02843 mode=40; 02844 } 02845 } else if(mode==40) { 02846 if(LIF||LIB) { 02847 mode=42; 02848 } 02849 02850 if(counts==false&&g[0]==0) { 02851 cros++; 02852 counts=true; 02853 } 02854 if(counts==true&&g[0]==99) { 02855 counts=false; 02856 } 02857 02858 if(cros==2) { 02859 // mode=41; 02860 //count=0; 02861 //cros=0; 02862 }//ゆっくりモードに入れなかった時の保険 02863 02864 switch(g[3]) { 02865 case 98: 02866 motor[TIRE_FR].pwm = normal; 02867 motor[TIRE_FR].dir = BACK; 02868 motor[TIRE_FL].pwm = normal; 02869 motor[TIRE_FL].dir = BACK; 02870 motor[TIRE_BR].pwm = normal; 02871 motor[TIRE_BR].dir = FOR; 02872 motor[TIRE_BL].pwm = normal; 02873 motor[TIRE_BL].dir = FOR; 02874 if(count>20000) { 02875 cross++; 02876 count=0; 02877 } 02878 mtc2=true; 02879 if(cross==1) { 02880 mode=41; 02881 count=0; 02882 cross=0; 02883 } 02884 break; 02885 case 0: 02886 motor[TIRE_FR].pwm = fast; 02887 motor[TIRE_FR].dir = BACK; 02888 motor[TIRE_FL].pwm = fast; 02889 motor[TIRE_FL].dir = BACK; 02890 motor[TIRE_BR].pwm = fast; 02891 motor[TIRE_BR].dir = FOR; 02892 motor[TIRE_BL].pwm = fast; 02893 motor[TIRE_BL].dir = FOR; 02894 mtc2=true; 02895 break; 02896 case 255: 02897 motor[TIRE_FR].pwm = slow; 02898 motor[TIRE_FR].dir = BACK; 02899 motor[TIRE_FL].pwm = fast; 02900 motor[TIRE_FL].dir = BACK; 02901 motor[TIRE_BR].pwm = fast; 02902 motor[TIRE_BR].dir = FOR; 02903 motor[TIRE_BL].pwm = slow; 02904 motor[TIRE_BL].dir = FOR; 02905 mtc2=true; 02906 break; 02907 case 253: 02908 motor[TIRE_FR].pwm = 0; 02909 motor[TIRE_FR].dir = FREE; 02910 motor[TIRE_FL].pwm = slow; 02911 motor[TIRE_FL].dir = BACK; 02912 motor[TIRE_BR].pwm = slow; 02913 motor[TIRE_BR].dir = FOR; 02914 motor[TIRE_BL].pwm = 0; 02915 motor[TIRE_BL].dir = FREE; 02916 mtc2=true; 02917 break; 02918 case 254: 02919 motor[TIRE_FR].pwm = 0; 02920 motor[TIRE_FR].dir = FREE; 02921 motor[TIRE_FL].pwm = normal; 02922 motor[TIRE_FL].dir = BACK; 02923 motor[TIRE_BR].pwm = normal; 02924 motor[TIRE_BR].dir = FOR; 02925 motor[TIRE_BL].pwm = 0; 02926 motor[TIRE_BL].dir = FREE; 02927 mtc2=true; 02928 break; 02929 case 1: 02930 motor[TIRE_FR].pwm = fast; 02931 motor[TIRE_FR].dir = BACK; 02932 motor[TIRE_FL].pwm = slow; 02933 motor[TIRE_FL].dir = BACK; 02934 motor[TIRE_BR].pwm = slow; 02935 motor[TIRE_BR].dir = FOR; 02936 motor[TIRE_BL].pwm = fast; 02937 motor[TIRE_BL].dir = FOR; 02938 mtc2=true; 02939 break; 02940 case 3: 02941 motor[TIRE_FR].pwm = slow; 02942 motor[TIRE_FR].dir = BACK; 02943 motor[TIRE_FL].pwm = 0; 02944 motor[TIRE_FL].dir = FREE; 02945 motor[TIRE_BR].pwm = 0; 02946 motor[TIRE_BR].dir = FREE; 02947 motor[TIRE_BL].pwm = slow; 02948 motor[TIRE_BL].dir = FOR; 02949 mtc2=true; 02950 break; 02951 case 2: 02952 motor[TIRE_FR].pwm = normal; 02953 motor[TIRE_FR].dir = BACK; 02954 motor[TIRE_FL].pwm = 0; 02955 motor[TIRE_FL].dir = FREE; 02956 motor[TIRE_BR].pwm = 0; 02957 motor[TIRE_BR].dir = FREE; 02958 motor[TIRE_BL].pwm = normal; 02959 motor[TIRE_BL].dir = FOR; 02960 mtc2=true; 02961 break; 02962 } 02963 if(g[2]!=98&&mtc2==true) { 02964 switch(g[2]) { 02965 case 0: 02966 mtc2=false; 02967 break; 02968 case 255: 02969 motor[TIRE_FR].pwm += 5; 02970 mtc2=false; 02971 break; 02972 case 253: 02973 motor[TIRE_FR].pwm += 10; 02974 mtc2=false; 02975 break; 02976 case 254: 02977 motor[TIRE_FR].pwm += 20; 02978 motor[TIRE_BR].pwm = 0; 02979 mtc2=false; 02980 break; 02981 case 1: 02982 motor[TIRE_BR].pwm += 5; 02983 mtc2=false; 02984 break; 02985 case 3: 02986 motor[TIRE_BR].pwm += 10; 02987 mtc2=false; 02988 break; 02989 case 2: 02990 motor[TIRE_BR].pwm += 20; 02991 motor[TIRE_FR].pwm = 0; 02992 mtc2=false; 02993 break; 02994 } 02995 } 02996 } else if(mode==41) { 02997 if(LIF||LIB) { 02998 mode=42; 02999 } 03000 motor[TIRE_FR].pwm = 30; 03001 motor[TIRE_FR].dir = BACK; 03002 motor[TIRE_FL].pwm = 0; 03003 motor[TIRE_FL].dir = FREE; 03004 motor[TIRE_BR].pwm = 0; 03005 motor[TIRE_BR].dir = FREE; 03006 motor[TIRE_BL].pwm = 30; 03007 motor[TIRE_BL].dir = FOR; 03008 if(g[1]==0) { 03009 count=0; 03010 mode=70; 03011 normal=60; 03012 fast=80; 03013 slow=40; 03014 } 03015 } else if(mode==42) { 03016 motor[TIRE_FR].pwm = 0; 03017 motor[TIRE_FR].dir = FREE; 03018 motor[TIRE_FL].pwm = 30; 03019 motor[TIRE_FL].dir = FOR; 03020 motor[TIRE_BR].pwm = 30; 03021 motor[TIRE_BR].dir = BACK; 03022 motor[TIRE_BL].pwm = 0; 03023 motor[TIRE_BL].dir = FREE; 03024 if(g[0]==0) { 03025 count=0; 03026 mode=70; 03027 normal=60; 03028 fast=80; 03029 slow=40; 03030 } 03031 03032 } else if(mode==70) 03033 switch(g[0]) { 03034 case 99: 03035 if(count>=100000) { 03036 cross++; 03037 mode=100; 03038 count=0; 03039 } 03040 if(cross==2) { 03041 cross=0; 03042 mode=100; 03043 } 03044 break; 03045 case 98: 03046 motor[TIRE_FR].pwm = normal; 03047 motor[TIRE_FR].dir = BACK; 03048 motor[TIRE_FL].pwm = normal; 03049 motor[TIRE_FL].dir = FOR; 03050 motor[TIRE_BR].pwm = normal; 03051 motor[TIRE_BR].dir = BACK; 03052 motor[TIRE_BL].pwm = normal; 03053 motor[TIRE_BL].dir = FOR; 03054 break; 03055 case 0: 03056 motor[TIRE_FR].pwm = fast; 03057 motor[TIRE_FR].dir = BACK; 03058 motor[TIRE_FL].pwm = fast; 03059 motor[TIRE_FL].dir = FOR; 03060 motor[TIRE_BR].pwm = fast; 03061 motor[TIRE_BR].dir = BACK; 03062 motor[TIRE_BL].pwm = fast; 03063 motor[TIRE_BL].dir = FOR; 03064 mtc=true; 03065 break; 03066 case 255: 03067 motor[TIRE_FR].pwm = fast; 03068 motor[TIRE_FR].dir = BACK; 03069 motor[TIRE_FL].pwm = slow; 03070 motor[TIRE_FL].dir = FOR; 03071 motor[TIRE_BR].pwm = slow; 03072 motor[TIRE_BR].dir = BACK; 03073 motor[TIRE_BL].pwm = fast; 03074 motor[TIRE_BL].dir = FOR; 03075 mtc=true; 03076 break; 03077 case 253: 03078 motor[TIRE_FR].pwm = slow; 03079 motor[TIRE_FR].dir = BACK; 03080 motor[TIRE_FL].pwm = 0; 03081 motor[TIRE_FL].dir = FREE; 03082 motor[TIRE_BR].pwm = 0; 03083 motor[TIRE_BR].dir = FREE; 03084 motor[TIRE_BL].pwm = slow; 03085 motor[TIRE_BL].dir = FOR; 03086 mtc=true; 03087 break; 03088 case 254: 03089 motor[TIRE_FR].pwm = normal; 03090 motor[TIRE_FR].dir = BACK; 03091 motor[TIRE_FL].pwm = 0; 03092 motor[TIRE_FL].dir = FREE; 03093 motor[TIRE_BR].pwm = 0; 03094 motor[TIRE_BR].dir = FREE; 03095 motor[TIRE_BL].pwm = normal; 03096 motor[TIRE_BL].dir = FOR; 03097 mtc=true; 03098 break; 03099 case 1: 03100 motor[TIRE_FR].pwm = slow; 03101 motor[TIRE_FR].dir = BACK; 03102 motor[TIRE_FL].pwm = fast; 03103 motor[TIRE_FL].dir = FOR; 03104 motor[TIRE_BR].pwm = fast; 03105 motor[TIRE_BR].dir = BACK; 03106 motor[TIRE_BL].pwm = slow; 03107 motor[TIRE_BL].dir = FOR; 03108 mtc=true; 03109 break; 03110 case 3: 03111 motor[TIRE_FR].pwm = 0; 03112 motor[TIRE_FR].dir = FREE; 03113 motor[TIRE_FL].pwm = slow; 03114 motor[TIRE_FL].dir = FOR; 03115 motor[TIRE_BR].pwm = slow; 03116 motor[TIRE_BR].dir = BACK; 03117 motor[TIRE_BL].pwm = 0; 03118 motor[TIRE_BL].dir = FREE; 03119 mtc=true; 03120 break; 03121 case 2: 03122 motor[TIRE_FR].pwm = 0; 03123 motor[TIRE_FR].dir = FREE; 03124 motor[TIRE_FL].pwm = normal; 03125 motor[TIRE_FL].dir = FOR; 03126 motor[TIRE_BR].pwm = normal; 03127 motor[TIRE_BR].dir = BACK; 03128 motor[TIRE_BL].pwm = 0; 03129 motor[TIRE_BL].dir = FREE; 03130 mtc=true; 03131 break; 03132 } 03133 if(g[1]!=98&&mtc==true) { 03134 switch(g[1]) { 03135 case 0: 03136 mtc=false; 03137 break; 03138 case 255: 03139 motor[TIRE_BR].pwm += 5; 03140 mtc=false; 03141 break; 03142 case 253: 03143 motor[TIRE_BR].pwm += 10; 03144 mtc=false; 03145 break; 03146 case 254: 03147 motor[TIRE_BR].pwm += 20; 03148 motor[TIRE_BL].pwm = 0; 03149 mtc=false; 03150 break; 03151 case 1: 03152 motor[TIRE_BL].pwm += 5; 03153 mtc=false; 03154 break; 03155 case 3: 03156 motor[TIRE_BL].pwm += 10; 03157 mtc=false; 03158 break; 03159 case 2: 03160 motor[TIRE_BL].pwm += 20; 03161 motor[TIRE_BR].pwm = 0; 03162 mtc=false; 03163 break; 03164 } 03165 } else if(mode==100) { 03166 motor[TIRE_FR].pwm = 20; 03167 motor[TIRE_FR].dir = BACK; 03168 motor[TIRE_FL].pwm = 20; 03169 motor[TIRE_FL].dir = FOR; 03170 motor[TIRE_BR].pwm = 23; 03171 motor[TIRE_BR].dir = BACK; 03172 motor[TIRE_BL].pwm = 20; 03173 motor[TIRE_BL].dir = FOR; 03174 if(StertSW) { 03175 SW_flag = true; 03176 } 03177 if(SW_flag ==true) { 03178 motor[TIRE_FR].pwm = 100; 03179 motor[TIRE_FR].dir = BRAKE; 03180 motor[TIRE_FL].pwm = 100; 03181 motor[TIRE_FL].dir = BRAKE; 03182 motor[TIRE_BR].pwm = 100; 03183 motor[TIRE_BR].dir = BRAKE; 03184 motor[TIRE_BL].pwm = 100; 03185 motor[TIRE_BL].dir = BRAKE; 03186 fast=60; 03187 normal=40; 03188 slow=20; 03189 mode =1; 03190 cross=0;//十字数える用 03191 cros=0; 03192 count=100000;//wait代替え 03193 UP_flag = false; 03194 SW_flag = false; 03195 Air_flag = false; 03196 zyouge=false; 03197 zyougedo=false; 03198 dz1=true; 03199 dz1i=false; 03200 dz2=true; 03201 dz2i=false; 03202 dz3=true; 03203 dz3i=false; 03204 dz4=true; 03205 dz4i=false; 03206 through=false; 03207 counts=false; 03208 mtc=false; 03209 mtc2 = false; 03210 current = 0; 03211 if(StertSW==false) { 03212 SW_flag = false; 03213 } 03214 } 03215 } 03216 03217 03218 03219 } 03220 #endif 03221 03222 #if USE_PROCESS_NUM>6 03223 static void Process6() 03224 { 03225 03226 } 03227 #endif 03228 03229 #if USE_PROCESS_NUM>7 03230 static void Process7() 03231 { 03232 03233 } 03234 #endif 03235 03236 #if USE_PROCESS_NUM>8 03237 static void Process8() 03238 { 03239 03240 } 03241 #endif 03242 03243 #if USE_PROCESS_NUM>9 03244 static void Process9() 03245 { 03246 03247 } 03248 #endif 03249 #endif 03250 #pragma endregion PROCESS 03251 03252 static void AllActuatorReset() 03253 { 03254 03255 #ifdef USE_SOLENOID 03256 solenoid.all = ALL_SOLENOID_OFF; 03257 #endif 03258 03259 #ifdef USE_MOTOR 03260 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 03261 motor[i].dir = FREE; 03262 motor[i].pwm = 0; 03263 } 03264 #endif 03265 } 03266 03267 /* 03268 void BuzzerTimer_func() 03269 { 03270 buzzer = !buzzer; 03271 //LED_DEBUG0 = !LED_DEBUG0; 03272 } 03273 */ 03274 void TapeLedEms_func() 03275 { 03276 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 03277 } 03278 03279 #pragma region USER-DEFINED-FUNCTIONS 03280 03281 03282 #pragma endregion
Generated on Mon Jul 18 2022 16:34:40 by
1.7.2