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