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