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