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 //スタート後どれくらい無検知でうごくか↓ 01310 if(g[2]==0&&count>=100000) { 01311 count=100000; 01312 cross=0; 01313 mode=3; 01314 } 01315 } else if(mode==3) { //横ライントレースから縦ライントレースへ 01316 motor[TIRE_FR].pwm = 30; 01317 motor[TIRE_FR].dir = FOR; 01318 motor[TIRE_FL].pwm = 0; 01319 motor[TIRE_FL].dir = FREE; 01320 motor[TIRE_BR].pwm = 0; 01321 motor[TIRE_BR].dir = FREE; 01322 motor[TIRE_BL].pwm = 30; 01323 motor[TIRE_BL].dir = BACK; 01324 if(g[1]==0) { 01325 mode=10; 01326 count=100000; 01327 } 01328 } else if(mode==10) { //縦ライントレース 01329 01330 if(counts==false&&g[2]==0) { 01331 cros++; 01332 counts=true; 01333 } 01334 if(counts==true&&g[2]==99) { 01335 counts=false; 01336 } 01337 01338 if(cros==Twsh) { 01339 mode=11; 01340 count=0; 01341 cros=0; 01342 }//ゆっくりモードに入れなかった時の保険 01343 switch(g[1]) { 01344 case 98: 01345 motor[TIRE_FR].pwm = normal; 01346 motor[TIRE_FR].dir = FOR; 01347 motor[TIRE_FL].pwm = normal; 01348 motor[TIRE_FL].dir = BACK; 01349 motor[TIRE_BR].pwm = normal; 01350 motor[TIRE_BR].dir = FOR; 01351 motor[TIRE_BL].pwm = normal; 01352 motor[TIRE_BL].dir = BACK; 01353 mtc=true; 01354 if(count>100000) { 01355 cross++; 01356 count=0; 01357 } 01358 if(cross==Twsh) { 01359 mode=11; 01360 count=0; 01361 cross=0; 01362 } 01363 break; 01364 case 0: 01365 motor[TIRE_FR].pwm = fast; 01366 motor[TIRE_FR].dir = FOR; 01367 motor[TIRE_FL].pwm = fast; 01368 motor[TIRE_FL].dir = BACK; 01369 motor[TIRE_BR].pwm = fast; 01370 motor[TIRE_BR].dir = FOR; 01371 motor[TIRE_BL].pwm = fast; 01372 motor[TIRE_BL].dir = BACK; 01373 mtc=true; 01374 break; 01375 case 255: 01376 motor[TIRE_FR].pwm = fast; 01377 motor[TIRE_FR].dir = FOR; 01378 motor[TIRE_FL].pwm = slow; 01379 motor[TIRE_FL].dir = BACK; 01380 motor[TIRE_BR].pwm = slow; 01381 motor[TIRE_BR].dir = FOR; 01382 motor[TIRE_BL].pwm = fast; 01383 motor[TIRE_BL].dir = BACK; 01384 mtc=true; 01385 break; 01386 case 253: 01387 motor[TIRE_FR].pwm = slow; 01388 motor[TIRE_FR].dir = FOR; 01389 motor[TIRE_FL].pwm = 0; 01390 motor[TIRE_FL].dir = FREE; 01391 motor[TIRE_BR].pwm = 0; 01392 motor[TIRE_BR].dir = FREE; 01393 motor[TIRE_BL].pwm = slow; 01394 motor[TIRE_BL].dir = BACK; 01395 mtc=true; 01396 break; 01397 case 254: 01398 motor[TIRE_FR].pwm = normal; 01399 motor[TIRE_FR].dir = FOR; 01400 motor[TIRE_FL].pwm = 0; 01401 motor[TIRE_FL].dir = FREE; 01402 motor[TIRE_BR].pwm = 0; 01403 motor[TIRE_BR].dir = FREE; 01404 motor[TIRE_BL].pwm = normal; 01405 motor[TIRE_BL].dir = BACK; 01406 mtc=true; 01407 break; 01408 case 1: 01409 motor[TIRE_FR].pwm = slow; 01410 motor[TIRE_FR].dir = FOR; 01411 motor[TIRE_FL].pwm = fast; 01412 motor[TIRE_FL].dir = BACK; 01413 motor[TIRE_BR].pwm = fast; 01414 motor[TIRE_BR].dir = FOR; 01415 motor[TIRE_BL].pwm = slow; 01416 motor[TIRE_BL].dir = BACK; 01417 mtc=true; 01418 break; 01419 case 3: 01420 motor[TIRE_FR].pwm = 0; 01421 motor[TIRE_FR].dir = FREE; 01422 motor[TIRE_FL].pwm = slow; 01423 motor[TIRE_FL].dir = BACK; 01424 motor[TIRE_BR].pwm = slow; 01425 motor[TIRE_BR].dir = FOR; 01426 motor[TIRE_BL].pwm = 0; 01427 motor[TIRE_BL].dir = FREE; 01428 mtc=true; 01429 break; 01430 case 2: 01431 motor[TIRE_FR].pwm = 0; 01432 motor[TIRE_FR].dir = FREE; 01433 motor[TIRE_FL].pwm = normal; 01434 motor[TIRE_FL].dir = BACK; 01435 motor[TIRE_BR].pwm = normal; 01436 motor[TIRE_BR].dir = FOR; 01437 motor[TIRE_BL].pwm = 0; 01438 motor[TIRE_BL].dir = FREE; 01439 mtc=true; 01440 break; 01441 } 01442 if(g[0]!=98&&mtc==true) { 01443 switch(g[0]) { 01444 case 0: 01445 mtc=false; 01446 break; 01447 case 255: 01448 motor[TIRE_FL].pwm += 5; 01449 mtc=false; 01450 break; 01451 case 253: 01452 motor[TIRE_FL].pwm += 10; 01453 mtc=false; 01454 break; 01455 case 254: 01456 motor[TIRE_FL].pwm += 20; 01457 motor[TIRE_FR].pwm = 0; 01458 mtc=false; 01459 break; 01460 case 1: 01461 motor[TIRE_FR].pwm += 5; 01462 mtc=false; 01463 break; 01464 case 3: 01465 motor[TIRE_FR].pwm += 10; 01466 mtc=false; 01467 break; 01468 case 2: 01469 motor[TIRE_FR].pwm += 20; 01470 motor[TIRE_FL].pwm = 0; 01471 mtc=false; 01472 break; 01473 } 01474 } 01475 } else if(mode==11) { 01476 motor[TIRE_FR].pwm = normal; 01477 motor[TIRE_FR].dir = FOR; 01478 motor[TIRE_FL].pwm = 0; 01479 motor[TIRE_FL].dir = FREE; 01480 motor[TIRE_BR].pwm = 0; 01481 motor[TIRE_BR].dir = FREE; 01482 motor[TIRE_BL].pwm = normal; 01483 motor[TIRE_BL].dir = BACK; 01484 if(g[2]==0) { 01485 count=0; 01486 cross=0; 01487 mode=20; 01488 } 01489 } else if(mode==20) { 01490 01491 if(Twsh==2) { 01492 solenoid.solenoid2 = SOLENOID_ON; 01493 } 01494 01495 if(counts==false&&g[1]==0) { 01496 cros++; 01497 counts=true; 01498 } 01499 if(counts==true&&g[1]==99) { 01500 counts=false; 01501 } 01502 01503 if(cros==2) { 01504 //mode=21; 01505 //count=0; 01506 //cros=0; 01507 }//ゆっくりモードに入れなかった時の保険 01508 if(g[1]==0) { 01509 fast=35; 01510 normal=20; 01511 slow=15; 01512 } 01513 switch(g[2]) { 01514 case 98: 01515 motor[TIRE_FR].pwm = normal; 01516 motor[TIRE_FR].dir = FOR; 01517 motor[TIRE_FL].pwm = normal; 01518 motor[TIRE_FL].dir = FOR; 01519 motor[TIRE_BR].pwm = normal; 01520 motor[TIRE_BR].dir = BACK; 01521 motor[TIRE_BL].pwm = normal; 01522 motor[TIRE_BL].dir = BACK; 01523 if(count>50000) { 01524 cross++; 01525 count=0; 01526 } 01527 mtc2=true; 01528 if(cross==2) { 01529 mode=22; 01530 cross=0; 01531 count=0; 01532 fast=40; 01533 normal=30; 01534 slow=20; 01535 } 01536 break; 01537 case 0: 01538 motor[TIRE_FR].pwm = fast; 01539 motor[TIRE_FR].dir = FOR; 01540 motor[TIRE_FL].pwm = fast; 01541 motor[TIRE_FL].dir = FOR; 01542 motor[TIRE_BR].pwm = fast; 01543 motor[TIRE_BR].dir = BACK; 01544 motor[TIRE_BL].pwm = fast; 01545 motor[TIRE_BL].dir = BACK; 01546 mtc2=true; 01547 break; 01548 case 255: 01549 motor[TIRE_FR].pwm = normal; 01550 motor[TIRE_FR].dir = FOR; 01551 motor[TIRE_FL].pwm = fast; 01552 motor[TIRE_FL].dir = FOR; 01553 motor[TIRE_BR].pwm = fast; 01554 motor[TIRE_BR].dir = BACK; 01555 motor[TIRE_BL].pwm = normal; 01556 motor[TIRE_BL].dir = BACK; 01557 mtc2=true; 01558 break; 01559 case 253: 01560 motor[TIRE_FR].pwm = 0; 01561 motor[TIRE_FR].dir = FREE; 01562 motor[TIRE_FL].pwm = slow; 01563 motor[TIRE_FL].dir = FOR; 01564 motor[TIRE_BR].pwm = slow; 01565 motor[TIRE_BR].dir = BACK; 01566 motor[TIRE_BL].pwm = 0; 01567 motor[TIRE_BL].dir = FREE; 01568 mtc2=true; 01569 break; 01570 case 254: 01571 motor[TIRE_FR].pwm = 0; 01572 motor[TIRE_FR].dir = FREE; 01573 motor[TIRE_FL].pwm = normal; 01574 motor[TIRE_FL].dir = FOR; 01575 motor[TIRE_BR].pwm = normal; 01576 motor[TIRE_BR].dir = BACK; 01577 motor[TIRE_BL].pwm = 0; 01578 motor[TIRE_BL].dir = FREE; 01579 mtc2=true; 01580 break; 01581 case 1: 01582 motor[TIRE_FR].pwm = fast; 01583 motor[TIRE_FR].dir = FOR; 01584 motor[TIRE_FL].pwm = slow; 01585 motor[TIRE_FL].dir = FOR; 01586 motor[TIRE_BR].pwm = slow; 01587 motor[TIRE_BR].dir = BACK; 01588 motor[TIRE_BL].pwm = fast; 01589 motor[TIRE_BL].dir = BACK; 01590 mtc2=true; 01591 break; 01592 case 3: 01593 motor[TIRE_FR].pwm = slow; 01594 motor[TIRE_FR].dir = FOR; 01595 motor[TIRE_FL].pwm = 0; 01596 motor[TIRE_FL].dir = FREE; 01597 motor[TIRE_BR].pwm = 0; 01598 motor[TIRE_BR].dir = FREE; 01599 motor[TIRE_BL].pwm = slow; 01600 motor[TIRE_BL].dir = BACK; 01601 mtc2=true; 01602 break; 01603 case 2: 01604 motor[TIRE_FR].pwm = normal; 01605 motor[TIRE_FR].dir = FOR; 01606 motor[TIRE_FL].pwm = 0; 01607 motor[TIRE_FL].dir = FREE; 01608 motor[TIRE_BR].pwm = 0; 01609 motor[TIRE_BR].dir = FREE; 01610 motor[TIRE_BL].pwm = normal; 01611 motor[TIRE_BL].dir = BACK; 01612 mtc2=true; 01613 break; 01614 } 01615 if(g[2]!=98&&mtc2==true) { 01616 switch(g[3]) { 01617 case 0: 01618 mtc2=false; 01619 break; 01620 case 255: 01621 motor[TIRE_BL].pwm += 5; 01622 mtc2=false; 01623 break; 01624 case 253: 01625 motor[TIRE_BL].pwm += 10; 01626 mtc2=false; 01627 break; 01628 case 254: 01629 motor[TIRE_BL].pwm += 20; 01630 motor[TIRE_FL].pwm = 0; 01631 mtc2=false; 01632 break; 01633 case 1: 01634 motor[TIRE_FL].pwm += 5; 01635 mtc2=false; 01636 break; 01637 case 3: 01638 motor[TIRE_FL].pwm += 10; 01639 mtc2=false; 01640 break; 01641 case 2: 01642 motor[TIRE_FL].pwm += 20; 01643 motor[TIRE_BL].pwm = 0; 01644 mtc2=false; 01645 break; 01646 } 01647 } 01648 01649 } else if(mode==21) { 01650 motor[TIRE_FR].pwm = 30; 01651 motor[TIRE_FR].dir = BRAKE; 01652 motor[TIRE_FL].pwm = 30; 01653 motor[TIRE_FL].dir = BRAKE; 01654 motor[TIRE_BR].pwm = 30; 01655 motor[TIRE_BR].dir = BRAKE; 01656 motor[TIRE_BL].pwm = 30; 01657 motor[TIRE_BL].dir = BRAKE; 01658 if(count<=100000) { 01659 mode=22; 01660 } 01661 } 01662 kiri=true; 01663 if(kiri) { 01664 if(mode==22) { 01665 motor[TIRE_FR].pwm = 100; 01666 motor[TIRE_FR].dir = BRAKE; 01667 motor[TIRE_FL].pwm = 100; 01668 motor[TIRE_FL].dir = BRAKE; 01669 motor[TIRE_BR].pwm = 100; 01670 motor[TIRE_BR].dir = BRAKE; 01671 motor[TIRE_BL].pwm = 100; 01672 motor[TIRE_BL].dir = BRAKE; 01673 if(count>=10000) { 01674 mode=23; 01675 } 01676 } else if(mode==23) { 01677 motor[TIRE_FR].pwm = 20; 01678 motor[TIRE_FR].dir = BACK; 01679 motor[TIRE_FL].pwm = 20; 01680 motor[TIRE_FL].dir = BACK; 01681 motor[TIRE_BR].pwm = 20; 01682 motor[TIRE_BR].dir = FOR; 01683 motor[TIRE_BL].pwm = 20; 01684 motor[TIRE_BL].dir = FOR; 01685 if(g[2]==98) { 01686 mode=24; 01687 count=0; 01688 } 01689 } else if(mode==24) { 01690 motor[TIRE_FR].pwm = 20; 01691 motor[TIRE_FR].dir = BACK; 01692 motor[TIRE_FL].pwm = 20; 01693 motor[TIRE_FL].dir = BACK; 01694 motor[TIRE_BR].pwm = 20; 01695 motor[TIRE_BR].dir = FOR; 01696 motor[TIRE_BL].pwm = 20; 01697 motor[TIRE_BL].dir = FOR; 01698 if(g[2]!=98) { 01699 count=0; 01700 mode=25; 01701 } 01702 } else if(mode==25) { 01703 motor[TIRE_FR].pwm = 10; 01704 motor[TIRE_FR].dir = BRAKE; 01705 motor[TIRE_FL].pwm = 10; 01706 motor[TIRE_FL].dir = BRAKE; 01707 motor[TIRE_BR].pwm = 10; 01708 motor[TIRE_BR].dir = BRAKE; 01709 motor[TIRE_BL].pwm = 10; 01710 motor[TIRE_BL].dir = BRAKE; 01711 if(count>=10000) { 01712 mode=26; 01713 count=0; 01714 } 01715 } else if(mode==26) { 01716 motor[TIRE_FR].pwm = 20; 01717 motor[TIRE_FR].dir = FOR; 01718 motor[TIRE_FL].pwm = 20; 01719 motor[TIRE_FL].dir = FOR; 01720 motor[TIRE_BR].pwm = 20; 01721 motor[TIRE_BR].dir = BACK; 01722 motor[TIRE_BL].pwm = 20; 01723 motor[TIRE_BL].dir = BACK; 01724 if(count>=5000) { 01725 mode=27; 01726 count=0; 01727 } 01728 } else if(mode==27) { 01729 motor[TIRE_FR].pwm = 20; 01730 motor[TIRE_FR].dir = FOR; 01731 motor[TIRE_FL].pwm = 20; 01732 motor[TIRE_FL].dir = FOR; 01733 motor[TIRE_BR].pwm = 20; 01734 motor[TIRE_BR].dir = FOR; 01735 motor[TIRE_BL].pwm = 20; 01736 motor[TIRE_BL].dir = FOR; 01737 //どれくらい回転するかって値 01738 if(count>=4000) { 01739 count=0; 01740 mode=28; 01741 } 01742 } else if(mode==28) { 01743 motor[TIRE_FR].pwm = 10; 01744 motor[TIRE_FR].dir = BRAKE; 01745 motor[TIRE_FL].pwm = 10; 01746 motor[TIRE_FL].dir = BRAKE; 01747 motor[TIRE_BR].pwm = 10; 01748 motor[TIRE_BR].dir = BRAKE; 01749 motor[TIRE_BL].pwm = 10; 01750 motor[TIRE_BL].dir = BRAKE; 01751 if(count>=10000) { 01752 mode=30; 01753 count=0; 01754 } 01755 } 01756 } else { 01757 if(mode==22) { 01758 motor[TIRE_FR].pwm = 0; 01759 motor[TIRE_FR].dir = FREE; 01760 motor[TIRE_FL].pwm = 25; 01761 motor[TIRE_FL].dir = FOR; 01762 motor[TIRE_BR].pwm = 25; 01763 motor[TIRE_BR].dir = BACK; 01764 motor[TIRE_BL].pwm = 0; 01765 motor[TIRE_BL].dir = FREE; 01766 if(g[0]==0&&count>=10000) { 01767 mode=30; 01768 count=0; 01769 cros=0; 01770 } 01771 } 01772 } 01773 if(mode==30) { 01774 motor[TIRE_FR].pwm = 60; 01775 motor[TIRE_FR].dir = FOR; 01776 motor[TIRE_FL].pwm = 60; 01777 motor[TIRE_FL].dir = BACK; 01778 motor[TIRE_BR].pwm = 60; 01779 motor[TIRE_BR].dir = FOR; 01780 motor[TIRE_BL].pwm = 57; 01781 motor[TIRE_BL].dir = BACK; 01782 if(g[1]==98) { 01783 mode=31; 01784 count=0; 01785 } 01786 } else if(mode==31) { 01787 motor[TIRE_FR].pwm = 0; 01788 motor[TIRE_FR].dir = FREE; 01789 motor[TIRE_FL].pwm = 0; 01790 motor[TIRE_FL].dir = FREE; 01791 motor[TIRE_BR].pwm = 0; 01792 motor[TIRE_BR].dir = FREE; 01793 motor[TIRE_BL].pwm = 0; 01794 motor[TIRE_BL].dir = FREE; 01795 if(count>=5000) { 01796 mode=32; 01797 count=0; 01798 } 01799 } else if(mode==32) { 01800 motor[TIRE_FR].pwm = 10; 01801 motor[TIRE_FR].dir = BRAKE; 01802 motor[TIRE_FL].pwm = 10; 01803 motor[TIRE_FL].dir = BRAKE; 01804 motor[TIRE_BR].pwm = 10; 01805 motor[TIRE_BR].dir = BRAKE; 01806 motor[TIRE_BL].pwm = 10; 01807 motor[TIRE_BL].dir = BRAKE; 01808 if(count>=30000) { 01809 mode=33; 01810 count=0; 01811 } 01812 } else if(mode==33) { 01813 motor[TIRE_FR].pwm = 30; 01814 motor[TIRE_FR].dir = FOR; 01815 motor[TIRE_FL].pwm = 0; 01816 motor[TIRE_FL].dir = FREE; 01817 motor[TIRE_BR].pwm = 0; 01818 motor[TIRE_BR].dir = FREE; 01819 motor[TIRE_BL].pwm = 30; 01820 motor[TIRE_BL].dir = BACK; 01821 if(g[3]==0&&count>=20000) { 01822 count=0; 01823 if(Twsh==2) { 01824 solenoid.solenoid2 = SOLENOID_OFF; 01825 } 01826 mode=40; 01827 } 01828 } else if(mode==40) { 01829 if(LIB) { 01830 mode=42; 01831 } 01832 switch(g[3]) { 01833 case 98: 01834 motor[TIRE_FR].pwm = normal; 01835 motor[TIRE_FR].dir = BACK; 01836 motor[TIRE_FL].pwm = normal; 01837 motor[TIRE_FL].dir = BACK; 01838 motor[TIRE_BR].pwm = normal; 01839 motor[TIRE_BR].dir = FOR; 01840 motor[TIRE_BL].pwm = normal; 01841 motor[TIRE_BL].dir = FOR; 01842 if(count>=10000) { 01843 cross++; 01844 count=0; 01845 } 01846 mtc2=true; 01847 if(cross==2) { 01848 mode=41; 01849 count=0; 01850 cross=0; 01851 } 01852 break; 01853 case 0: 01854 motor[TIRE_FR].pwm = fast; 01855 motor[TIRE_FR].dir = BACK; 01856 motor[TIRE_FL].pwm = fast; 01857 motor[TIRE_FL].dir = BACK; 01858 motor[TIRE_BR].pwm = fast; 01859 motor[TIRE_BR].dir = FOR; 01860 motor[TIRE_BL].pwm = fast; 01861 motor[TIRE_BL].dir = FOR; 01862 mtc2=true; 01863 break; 01864 case 255: 01865 motor[TIRE_FR].pwm = slow; 01866 motor[TIRE_FR].dir = BACK; 01867 motor[TIRE_FL].pwm = fast; 01868 motor[TIRE_FL].dir = BACK; 01869 motor[TIRE_BR].pwm = fast; 01870 motor[TIRE_BR].dir = FOR; 01871 motor[TIRE_BL].pwm = slow; 01872 motor[TIRE_BL].dir = FOR; 01873 mtc2=true; 01874 break; 01875 case 253: 01876 motor[TIRE_FR].pwm = 0; 01877 motor[TIRE_FR].dir = FREE; 01878 motor[TIRE_FL].pwm = slow; 01879 motor[TIRE_FL].dir = BACK; 01880 motor[TIRE_BR].pwm = slow; 01881 motor[TIRE_BR].dir = FOR; 01882 motor[TIRE_BL].pwm = 0; 01883 motor[TIRE_BL].dir = FREE; 01884 mtc2=true; 01885 break; 01886 case 254: 01887 motor[TIRE_FR].pwm = 0; 01888 motor[TIRE_FR].dir = FREE; 01889 motor[TIRE_FL].pwm = normal; 01890 motor[TIRE_FL].dir = BACK; 01891 motor[TIRE_BR].pwm = normal; 01892 motor[TIRE_BR].dir = FOR; 01893 motor[TIRE_BL].pwm = 0; 01894 motor[TIRE_BL].dir = FREE; 01895 mtc2=true; 01896 break; 01897 case 1: 01898 motor[TIRE_FR].pwm = fast; 01899 motor[TIRE_FR].dir = BACK; 01900 motor[TIRE_FL].pwm = slow; 01901 motor[TIRE_FL].dir = BACK; 01902 motor[TIRE_BR].pwm = slow; 01903 motor[TIRE_BR].dir = FOR; 01904 motor[TIRE_BL].pwm = fast; 01905 motor[TIRE_BL].dir = FOR; 01906 mtc2=true; 01907 break; 01908 case 3: 01909 motor[TIRE_FR].pwm = slow; 01910 motor[TIRE_FR].dir = BACK; 01911 motor[TIRE_FL].pwm = 0; 01912 motor[TIRE_FL].dir = FREE; 01913 motor[TIRE_BR].pwm = 0; 01914 motor[TIRE_BR].dir = FREE; 01915 motor[TIRE_BL].pwm = slow; 01916 motor[TIRE_BL].dir = FOR; 01917 mtc2=true; 01918 break; 01919 case 2: 01920 motor[TIRE_FR].pwm = normal; 01921 motor[TIRE_FR].dir = BACK; 01922 motor[TIRE_FL].pwm = 0; 01923 motor[TIRE_FL].dir = FREE; 01924 motor[TIRE_BR].pwm = 0; 01925 motor[TIRE_BR].dir = FREE; 01926 motor[TIRE_BL].pwm = normal; 01927 motor[TIRE_BL].dir = FOR; 01928 mtc2=true; 01929 break; 01930 } 01931 if(g[2]!=98&&mtc2==true) { 01932 switch(g[2]) { 01933 case 0: 01934 mtc2=false; 01935 break; 01936 case 255: 01937 motor[TIRE_FR].pwm += 5; 01938 mtc2=false; 01939 break; 01940 case 253: 01941 motor[TIRE_FR].pwm += 10; 01942 mtc2=false; 01943 break; 01944 case 254: 01945 motor[TIRE_FR].pwm += 20; 01946 motor[TIRE_BR].pwm = 0; 01947 mtc2=false; 01948 break; 01949 case 1: 01950 motor[TIRE_BR].pwm += 5; 01951 mtc2=false; 01952 break; 01953 case 3: 01954 motor[TIRE_BR].pwm += 10; 01955 mtc2=false; 01956 break; 01957 case 2: 01958 motor[TIRE_BR].pwm += 20; 01959 motor[TIRE_FR].pwm = 0; 01960 mtc2=false; 01961 break; 01962 } 01963 } 01964 } else if(mode==41) { 01965 if(LIF||LIB) { 01966 mode=42; 01967 } 01968 motor[TIRE_FR].pwm = 30; 01969 motor[TIRE_FR].dir = BACK; 01970 motor[TIRE_FL].pwm = 0; 01971 motor[TIRE_FL].dir = FREE; 01972 motor[TIRE_BR].pwm = 0; 01973 motor[TIRE_BR].dir = FREE; 01974 motor[TIRE_BL].pwm = 30; 01975 motor[TIRE_BL].dir = FOR; 01976 if(g[1]==0) { 01977 count=0; 01978 mode=70; 01979 normal=60; 01980 fast=70; 01981 slow=40; 01982 } 01983 } else if(mode==42) { 01984 motor[TIRE_FR].pwm = 0; 01985 motor[TIRE_FR].dir = FREE; 01986 motor[TIRE_FL].pwm = 20; 01987 motor[TIRE_FL].dir = FOR; 01988 motor[TIRE_BR].pwm = 20; 01989 motor[TIRE_BR].dir = BACK; 01990 motor[TIRE_BL].pwm = 0; 01991 motor[TIRE_BL].dir = FREE; 01992 if(g[0]==0) { 01993 count=0; 01994 slow=40; 01995 } 01996 01997 } else if(mode==70) 01998 switch(g[0]) { 01999 case 99: 02000 if(count>=100000) { 02001 cross++; 02002 mode=100; 02003 count=0; 02004 } 02005 if(cross==2) { 02006 cross=0; 02007 mode=100; 02008 } 02009 break; 02010 case 98: 02011 motor[TIRE_FR].pwm = normal; 02012 motor[TIRE_FR].dir = BACK; 02013 motor[TIRE_FL].pwm = normal; 02014 motor[TIRE_FL].dir = FOR; 02015 motor[TIRE_BR].pwm = normal; 02016 motor[TIRE_BR].dir = BACK; 02017 motor[TIRE_BL].pwm = normal; 02018 motor[TIRE_BL].dir = FOR; 02019 break; 02020 case 0: 02021 motor[TIRE_FR].pwm = fast; 02022 motor[TIRE_FR].dir = BACK; 02023 motor[TIRE_FL].pwm = fast; 02024 motor[TIRE_FL].dir = FOR; 02025 motor[TIRE_BR].pwm = fast; 02026 motor[TIRE_BR].dir = BACK; 02027 motor[TIRE_BL].pwm = fast; 02028 motor[TIRE_BL].dir = FOR; 02029 mtc=true; 02030 break; 02031 case 255: 02032 motor[TIRE_FR].pwm = fast; 02033 motor[TIRE_FR].dir = BACK; 02034 motor[TIRE_FL].pwm = slow; 02035 motor[TIRE_FL].dir = FOR; 02036 motor[TIRE_BR].pwm = slow; 02037 motor[TIRE_BR].dir = BACK; 02038 motor[TIRE_BL].pwm = fast; 02039 motor[TIRE_BL].dir = FOR; 02040 mtc=true; 02041 break; 02042 case 253: 02043 motor[TIRE_FR].pwm = slow; 02044 motor[TIRE_FR].dir = BACK; 02045 motor[TIRE_FL].pwm = 0; 02046 motor[TIRE_FL].dir = FREE; 02047 motor[TIRE_BR].pwm = 0; 02048 motor[TIRE_BR].dir = FREE; 02049 motor[TIRE_BL].pwm = slow; 02050 motor[TIRE_BL].dir = FOR; 02051 mtc=true; 02052 break; 02053 case 254: 02054 motor[TIRE_FR].pwm = normal; 02055 motor[TIRE_FR].dir = BACK; 02056 motor[TIRE_FL].pwm = 0; 02057 motor[TIRE_FL].dir = FREE; 02058 motor[TIRE_BR].pwm = 0; 02059 motor[TIRE_BR].dir = FREE; 02060 motor[TIRE_BL].pwm = normal; 02061 motor[TIRE_BL].dir = FOR; 02062 mtc=true; 02063 break; 02064 case 1: 02065 motor[TIRE_FR].pwm = slow; 02066 motor[TIRE_FR].dir = BACK; 02067 motor[TIRE_FL].pwm = fast; 02068 motor[TIRE_FL].dir = FOR; 02069 motor[TIRE_BR].pwm = fast; 02070 motor[TIRE_BR].dir = BACK; 02071 motor[TIRE_BL].pwm = slow; 02072 motor[TIRE_BL].dir = FOR; 02073 mtc=true; 02074 break; 02075 case 3: 02076 motor[TIRE_FR].pwm = 0; 02077 motor[TIRE_FR].dir = FREE; 02078 motor[TIRE_FL].pwm = slow; 02079 motor[TIRE_FL].dir = FOR; 02080 motor[TIRE_BR].pwm = slow; 02081 motor[TIRE_BR].dir = BACK; 02082 motor[TIRE_BL].pwm = 0; 02083 motor[TIRE_BL].dir = FREE; 02084 mtc=true; 02085 break; 02086 case 2: 02087 motor[TIRE_FR].pwm = 0; 02088 motor[TIRE_FR].dir = FREE; 02089 motor[TIRE_FL].pwm = normal; 02090 motor[TIRE_FL].dir = FOR; 02091 motor[TIRE_BR].pwm = normal; 02092 motor[TIRE_BR].dir = BACK; 02093 motor[TIRE_BL].pwm = 0; 02094 motor[TIRE_BL].dir = FREE; 02095 mtc=true; 02096 break; 02097 } 02098 if(g[1]!=98&&mtc==true) { 02099 switch(g[1]) { 02100 case 0: 02101 mtc=false; 02102 break; 02103 case 255: 02104 motor[TIRE_BR].pwm += 5; 02105 mtc=false; 02106 break; 02107 case 253: 02108 motor[TIRE_BR].pwm += 10; 02109 mtc=false; 02110 break; 02111 case 254: 02112 motor[TIRE_BR].pwm += 20; 02113 motor[TIRE_BL].pwm = 0; 02114 mtc=false; 02115 break; 02116 case 1: 02117 motor[TIRE_BL].pwm += 5; 02118 mtc=false; 02119 break; 02120 case 3: 02121 motor[TIRE_BL].pwm += 10; 02122 mtc=false; 02123 break; 02124 case 2: 02125 motor[TIRE_BL].pwm += 20; 02126 motor[TIRE_BR].pwm = 0; 02127 mtc=false; 02128 break; 02129 } 02130 } else if(mode==100) { 02131 motor[TIRE_FR].pwm = 20; 02132 motor[TIRE_FR].dir = BACK; 02133 motor[TIRE_FL].pwm = 20; 02134 motor[TIRE_FL].dir = FOR; 02135 motor[TIRE_BR].pwm = 23; 02136 motor[TIRE_BR].dir = BACK; 02137 motor[TIRE_BL].pwm = 20; 02138 motor[TIRE_BL].dir = FOR; 02139 } 02140 } 02141 #endif 02142 02143 #if USE_PROCESS_NUM>5 02144 static void Process5() 02145 { 02146 } 02147 #endif 02148 02149 #if USE_PROCESS_NUM>6 02150 static void Process6() 02151 { 02152 // 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]); 02153 LedMode(3); 02154 count++; 02155 if(mode==1) {//スタートゾーンから白線検知 02156 fast=40; 02157 normal=30; 02158 slow=20; 02159 motor[TIRE_FR].pwm = normal; 02160 motor[TIRE_FR].dir = BACK; 02161 motor[TIRE_FL].pwm = normal; 02162 motor[TIRE_FL].dir = FOR; 02163 motor[TIRE_BR].pwm = normal; 02164 motor[TIRE_BR].dir = BACK; 02165 motor[TIRE_BL].pwm = normal; 02166 motor[TIRE_BL].dir = FOR; 02167 if(g[2]==0&&count>=100000) { 02168 count=100000; 02169 cross=0; 02170 mode=3; 02171 } 02172 } else if(mode==3) { //横ライントレースから縦ライントレースへ 02173 motor[TIRE_FR].pwm = 0; 02174 motor[TIRE_FR].dir = FREE; 02175 motor[TIRE_FL].pwm = 25; 02176 motor[TIRE_FL].dir = FOR; 02177 motor[TIRE_BR].pwm = 25; 02178 motor[TIRE_BR].dir = BACK; 02179 motor[TIRE_BL].pwm = 0; 02180 motor[TIRE_BL].dir = FREE; 02181 if(g[0]==0) { 02182 mode=10; 02183 count=100000; 02184 } 02185 } else if(mode==10) { //縦ライントレース 02186 if(counts==false&&g[2]==0) { 02187 cros++; 02188 counts=true; 02189 } 02190 if(counts==true&&g[2]==99) { 02191 counts=false; 02192 } 02193 02194 if(cros==Twsh) { 02195 mode=11; 02196 count=0; 02197 cros=0; 02198 }//ゆっくりモードに入れなかった時の保険 02199 switch(g[0]) { 02200 case 98: 02201 motor[TIRE_FR].pwm = normal; 02202 motor[TIRE_FR].dir = BACK; 02203 motor[TIRE_FL].pwm = normal; 02204 motor[TIRE_FL].dir = FOR; 02205 motor[TIRE_BR].pwm = normal; 02206 motor[TIRE_BR].dir = BACK; 02207 motor[TIRE_BL].pwm = normal; 02208 motor[TIRE_BL].dir = FOR; 02209 mtc=true; 02210 if(count>80000) { 02211 cross++; 02212 count=0; 02213 } 02214 if(cross==Twsh) { 02215 mode=11; 02216 count=0; 02217 cross=0; 02218 } 02219 break; 02220 case 0: 02221 motor[TIRE_FR].pwm = fast; 02222 motor[TIRE_FR].dir = BACK; 02223 motor[TIRE_FL].pwm = fast; 02224 motor[TIRE_FL].dir = FOR; 02225 motor[TIRE_BR].pwm = fast; 02226 motor[TIRE_BR].dir = BACK; 02227 motor[TIRE_BL].pwm = fast; 02228 motor[TIRE_BL].dir = FOR; 02229 mtc=true; 02230 break; 02231 case 255: 02232 motor[TIRE_FR].pwm = fast; 02233 motor[TIRE_FR].dir = BACK; 02234 motor[TIRE_FL].pwm = slow; 02235 motor[TIRE_FL].dir = FOR; 02236 motor[TIRE_BR].pwm = slow; 02237 motor[TIRE_BR].dir = BACK; 02238 motor[TIRE_BL].pwm = fast; 02239 motor[TIRE_BL].dir = FOR; 02240 mtc=true; 02241 break; 02242 case 253: 02243 motor[TIRE_FR].pwm = slow; 02244 motor[TIRE_FR].dir = BACK; 02245 motor[TIRE_FL].pwm = 0; 02246 motor[TIRE_FL].dir = FREE; 02247 motor[TIRE_BR].pwm = 0; 02248 motor[TIRE_BR].dir = FREE; 02249 motor[TIRE_BL].pwm = slow; 02250 motor[TIRE_BL].dir = FOR; 02251 mtc=true; 02252 break; 02253 case 254: 02254 motor[TIRE_FR].pwm = normal; 02255 motor[TIRE_FR].dir = BACK; 02256 motor[TIRE_FL].pwm = 0; 02257 motor[TIRE_FL].dir = FREE; 02258 motor[TIRE_BR].pwm = 0; 02259 motor[TIRE_BR].dir = FREE; 02260 motor[TIRE_BL].pwm = normal; 02261 motor[TIRE_BL].dir = FOR; 02262 mtc=true; 02263 break; 02264 case 1: 02265 motor[TIRE_FR].pwm = slow; 02266 motor[TIRE_FR].dir = BACK; 02267 motor[TIRE_FL].pwm = fast; 02268 motor[TIRE_FL].dir = FOR; 02269 motor[TIRE_BR].pwm = fast; 02270 motor[TIRE_BR].dir = BACK; 02271 motor[TIRE_BL].pwm = slow; 02272 motor[TIRE_BL].dir = FOR; 02273 mtc=true; 02274 break; 02275 case 3: 02276 motor[TIRE_FR].pwm = 0; 02277 motor[TIRE_FR].dir = FREE; 02278 motor[TIRE_FL].pwm = slow; 02279 motor[TIRE_FL].dir = FOR; 02280 motor[TIRE_BR].pwm = slow; 02281 motor[TIRE_BR].dir = BACK; 02282 motor[TIRE_BL].pwm = 0; 02283 motor[TIRE_BL].dir = FREE; 02284 mtc=true; 02285 break; 02286 case 2: 02287 motor[TIRE_FR].pwm = 0; 02288 motor[TIRE_FR].dir = FREE; 02289 motor[TIRE_FL].pwm = normal; 02290 motor[TIRE_FL].dir = FOR; 02291 motor[TIRE_BR].pwm = normal; 02292 motor[TIRE_BR].dir = BACK; 02293 motor[TIRE_BL].pwm = 0; 02294 motor[TIRE_BL].dir = FREE; 02295 mtc=true; 02296 break; 02297 } 02298 if(g[0]!=98&&mtc==true) { 02299 switch(g[1]) { 02300 case 0: 02301 mtc=false; 02302 break; 02303 case 255: 02304 motor[TIRE_BR].pwm += 5; 02305 mtc=false; 02306 break; 02307 case 253: 02308 motor[TIRE_BR].pwm += 10; 02309 mtc=false; 02310 break; 02311 case 254: 02312 motor[TIRE_BR].pwm += 20; 02313 motor[TIRE_BL].pwm = 0; 02314 mtc=false; 02315 break; 02316 case 1: 02317 motor[TIRE_BL].pwm += 5; 02318 mtc=false; 02319 break; 02320 case 3: 02321 motor[TIRE_BL].pwm += 10; 02322 mtc=false; 02323 break; 02324 case 2: 02325 motor[TIRE_BL].pwm += 20; 02326 motor[TIRE_BR].pwm = 0; 02327 mtc=false; 02328 break; 02329 } 02330 } 02331 } else if(mode==11) { 02332 motor[TIRE_FR].pwm = 0; 02333 motor[TIRE_FR].dir = FREE; 02334 motor[TIRE_FL].pwm = normal; 02335 motor[TIRE_FL].dir = FOR; 02336 motor[TIRE_BR].pwm = normal; 02337 motor[TIRE_BR].dir = BACK; 02338 motor[TIRE_BL].pwm = 0; 02339 motor[TIRE_BL].dir = FREE; 02340 if(g[2]==0) { 02341 count=0; 02342 cross=0; 02343 mode=20; 02344 } 02345 } else if(mode==20) { 02346 02347 if(Twsh==2) { 02348 solenoid.solenoid2 = SOLENOID_ON; 02349 } 02350 if(counts==false&&g[0]==0) { 02351 cros++; 02352 counts=true; 02353 } 02354 if(counts==true&&g[0]==99) { 02355 counts=false; 02356 } 02357 02358 if(cros==2) { 02359 02360 // mode=21; 02361 // count=0; 02362 // cros=0; 02363 }//ゆっくりモードに入れなかった時の保険 02364 if(g[0]==0) { 02365 fast=35; 02366 normal=20; 02367 slow=15; 02368 } 02369 switch(g[2]) { 02370 case 98: 02371 motor[TIRE_FR].pwm = normal; 02372 motor[TIRE_FR].dir = FOR; 02373 motor[TIRE_FL].pwm = normal; 02374 motor[TIRE_FL].dir = FOR; 02375 motor[TIRE_BR].pwm = normal; 02376 motor[TIRE_BR].dir = BACK; 02377 motor[TIRE_BL].pwm = normal; 02378 motor[TIRE_BL].dir = BACK; 02379 if(count>30000) { 02380 cross++; 02381 count=0; 02382 } 02383 mtc2=true; 02384 if(cross==2) { 02385 mode=22; 02386 count=0; 02387 cross=0; 02388 fast=40; 02389 normal=30; 02390 slow=20; 02391 } 02392 break; 02393 case 0: 02394 motor[TIRE_FR].pwm = fast; 02395 motor[TIRE_FR].dir = FOR; 02396 motor[TIRE_FL].pwm = fast; 02397 motor[TIRE_FL].dir = FOR; 02398 motor[TIRE_BR].pwm = fast; 02399 motor[TIRE_BR].dir = BACK; 02400 motor[TIRE_BL].pwm = fast; 02401 motor[TIRE_BL].dir = BACK; 02402 mtc2=true; 02403 break; 02404 case 255: 02405 motor[TIRE_FR].pwm = normal; 02406 motor[TIRE_FR].dir = FOR; 02407 motor[TIRE_FL].pwm = fast; 02408 motor[TIRE_FL].dir = FOR; 02409 motor[TIRE_BR].pwm = fast; 02410 motor[TIRE_BR].dir = BACK; 02411 motor[TIRE_BL].pwm = normal; 02412 motor[TIRE_BL].dir = BACK; 02413 mtc2=true; 02414 break; 02415 case 253: 02416 motor[TIRE_FR].pwm = 0; 02417 motor[TIRE_FR].dir = FREE; 02418 motor[TIRE_FL].pwm = slow; 02419 motor[TIRE_FL].dir = FOR; 02420 motor[TIRE_BR].pwm = slow; 02421 motor[TIRE_BR].dir = BACK; 02422 motor[TIRE_BL].pwm = 0; 02423 motor[TIRE_BL].dir = FREE; 02424 mtc2=true; 02425 break; 02426 case 254: 02427 motor[TIRE_FR].pwm = 0; 02428 motor[TIRE_FR].dir = FREE; 02429 motor[TIRE_FL].pwm = normal; 02430 motor[TIRE_FL].dir = FOR; 02431 motor[TIRE_BR].pwm = normal; 02432 motor[TIRE_BR].dir = BACK; 02433 motor[TIRE_BL].pwm = 0; 02434 motor[TIRE_BL].dir = FREE; 02435 mtc2=true; 02436 break; 02437 case 1: 02438 motor[TIRE_FR].pwm = fast; 02439 motor[TIRE_FR].dir = FOR; 02440 motor[TIRE_FL].pwm = slow; 02441 motor[TIRE_FL].dir = FOR; 02442 motor[TIRE_BR].pwm = slow; 02443 motor[TIRE_BR].dir = BACK; 02444 motor[TIRE_BL].pwm = fast; 02445 motor[TIRE_BL].dir = BACK; 02446 mtc2=true; 02447 break; 02448 case 3: 02449 motor[TIRE_FR].pwm = slow; 02450 motor[TIRE_FR].dir = FOR; 02451 motor[TIRE_FL].pwm = 0; 02452 motor[TIRE_FL].dir = FREE; 02453 motor[TIRE_BR].pwm = 0; 02454 motor[TIRE_BR].dir = FREE; 02455 motor[TIRE_BL].pwm = slow; 02456 motor[TIRE_BL].dir = BACK; 02457 mtc2=true; 02458 break; 02459 case 2: 02460 motor[TIRE_FR].pwm = normal; 02461 motor[TIRE_FR].dir = FOR; 02462 motor[TIRE_FL].pwm = 0; 02463 motor[TIRE_FL].dir = FREE; 02464 motor[TIRE_BR].pwm = 0; 02465 motor[TIRE_BR].dir = FREE; 02466 motor[TIRE_BL].pwm = normal; 02467 motor[TIRE_BL].dir = BACK; 02468 mtc2=true; 02469 break; 02470 } 02471 if(g[2]!=98&&mtc2==true) { 02472 switch(g[3]) { 02473 case 0: 02474 mtc2=false; 02475 break; 02476 case 255: 02477 motor[TIRE_BL].pwm += 5; 02478 mtc2=false; 02479 break; 02480 case 253: 02481 motor[TIRE_BL].pwm += 10; 02482 mtc2=false; 02483 break; 02484 case 254: 02485 motor[TIRE_BL].pwm += 20; 02486 motor[TIRE_FL].pwm = 0; 02487 mtc2=false; 02488 break; 02489 case 1: 02490 motor[TIRE_FL].pwm += 5; 02491 mtc2=false; 02492 break; 02493 case 3: 02494 motor[TIRE_FL].pwm += 10; 02495 mtc2=false; 02496 break; 02497 case 2: 02498 motor[TIRE_FL].pwm += 20; 02499 motor[TIRE_BL].pwm = 0; 02500 mtc2=false; 02501 break; 02502 } 02503 } 02504 } else if(mode==21) { 02505 motor[TIRE_FR].pwm = 30; 02506 motor[TIRE_FR].dir = FOR; 02507 motor[TIRE_FL].pwm = 30; 02508 motor[TIRE_FL].dir = FOR; 02509 motor[TIRE_BR].pwm = 30; 02510 motor[TIRE_BR].dir = BACK; 02511 motor[TIRE_BL].pwm = 30; 02512 motor[TIRE_BL].dir = BACK; 02513 if(g[2]==98&&count>=10000) { 02514 mode=22; 02515 count=0; 02516 } 02517 } 02518 kiri=true; 02519 if(kiri) { 02520 if(mode==22) { 02521 motor[TIRE_FR].pwm = 100; 02522 motor[TIRE_FR].dir = BRAKE; 02523 motor[TIRE_FL].pwm = 100; 02524 motor[TIRE_FL].dir = BRAKE; 02525 motor[TIRE_BR].pwm = 100; 02526 motor[TIRE_BR].dir = BRAKE; 02527 motor[TIRE_BL].pwm = 100; 02528 motor[TIRE_BL].dir = BRAKE; 02529 if(count>=10000) { 02530 mode=23; 02531 } 02532 } else if(mode==23) { 02533 motor[TIRE_FR].pwm = 20; 02534 motor[TIRE_FR].dir = BACK; 02535 motor[TIRE_FL].pwm = 20; 02536 motor[TIRE_FL].dir = BACK; 02537 motor[TIRE_BR].pwm = 20; 02538 motor[TIRE_BR].dir = FOR; 02539 motor[TIRE_BL].pwm = 20; 02540 motor[TIRE_BL].dir = FOR; 02541 if(g[2]==98) { 02542 mode=24; 02543 count=0; 02544 } 02545 } else if(mode==24) { 02546 motor[TIRE_FR].pwm = 20; 02547 motor[TIRE_FR].dir = BACK; 02548 motor[TIRE_FL].pwm = 20; 02549 motor[TIRE_FL].dir = BACK; 02550 motor[TIRE_BR].pwm = 20; 02551 motor[TIRE_BR].dir = FOR; 02552 motor[TIRE_BL].pwm = 20; 02553 motor[TIRE_BL].dir = FOR; 02554 if(g[2]!=98) { 02555 count=0; 02556 mode=25; 02557 } 02558 } else if(mode==25) { 02559 motor[TIRE_FR].pwm = 10; 02560 motor[TIRE_FR].dir = BRAKE; 02561 motor[TIRE_FL].pwm = 10; 02562 motor[TIRE_FL].dir = BRAKE; 02563 motor[TIRE_BR].pwm = 10; 02564 motor[TIRE_BR].dir = BRAKE; 02565 motor[TIRE_BL].pwm = 10; 02566 motor[TIRE_BL].dir = BRAKE; 02567 if(count>=10000) { 02568 mode=26; 02569 count=0; 02570 } 02571 } else if(mode==26) { 02572 motor[TIRE_FR].pwm = 20; 02573 motor[TIRE_FR].dir = FOR; 02574 motor[TIRE_FL].pwm = 20; 02575 motor[TIRE_FL].dir = FOR; 02576 motor[TIRE_BR].pwm = 20; 02577 motor[TIRE_BR].dir = BACK; 02578 motor[TIRE_BL].pwm = 20; 02579 motor[TIRE_BL].dir = BACK; 02580 if(count>=5000) { 02581 mode=27; 02582 count=0; 02583 } 02584 } else if(mode==27) { 02585 motor[TIRE_FR].pwm = 20; 02586 motor[TIRE_FR].dir = FOR; 02587 motor[TIRE_FL].pwm = 20; 02588 motor[TIRE_FL].dir = FOR; 02589 motor[TIRE_BR].pwm = 20; 02590 motor[TIRE_BR].dir = FOR; 02591 motor[TIRE_BL].pwm = 20; 02592 motor[TIRE_BL].dir = FOR; 02593 //どれくらい回転するかって値 02594 if(count>=100) { 02595 count=0; 02596 mode=30; 02597 } 02598 } else if(mode==24) { 02599 motor[TIRE_FR].pwm = 10; 02600 motor[TIRE_FR].dir = BRAKE; 02601 motor[TIRE_FL].pwm = 10; 02602 motor[TIRE_FL].dir = BRAKE; 02603 motor[TIRE_BR].pwm = 10; 02604 motor[TIRE_BR].dir = BRAKE; 02605 motor[TIRE_BL].pwm = 10; 02606 motor[TIRE_BL].dir = BRAKE; 02607 if(count>=10000) { 02608 mode=30; 02609 count=0; 02610 } 02611 } 02612 } else { 02613 if(mode==22) { 02614 motor[TIRE_FR].pwm = 0; 02615 motor[TIRE_FR].dir = FREE; 02616 motor[TIRE_FL].pwm = 25; 02617 motor[TIRE_FL].dir = FOR; 02618 motor[TIRE_BR].pwm = 25; 02619 motor[TIRE_BR].dir = BACK; 02620 motor[TIRE_BL].pwm = 0; 02621 motor[TIRE_BL].dir = FREE; 02622 if(g[0]==0&&count>=10000) { 02623 mode=30; 02624 count=0; 02625 cros=0; 02626 } 02627 } 02628 } 02629 if(mode==30) { 02630 motor[TIRE_FR].pwm = 59; 02631 motor[TIRE_FR].dir = BACK; 02632 motor[TIRE_FL].pwm = 62; 02633 motor[TIRE_FL].dir = FOR; 02634 motor[TIRE_BR].pwm = 59; 02635 motor[TIRE_BR].dir = BACK; 02636 motor[TIRE_BL].pwm = 62; 02637 motor[TIRE_BL].dir = FOR; 02638 if(g[0]==98) { 02639 mode=31; 02640 count=0; 02641 } 02642 } else if(mode==31) { 02643 motor[TIRE_FR].pwm = 0; 02644 motor[TIRE_FR].dir = FREE; 02645 motor[TIRE_FL].pwm = 0; 02646 motor[TIRE_FL].dir = FREE; 02647 motor[TIRE_BR].pwm = 0; 02648 motor[TIRE_BR].dir = FREE; 02649 motor[TIRE_BL].pwm = 0; 02650 motor[TIRE_BL].dir = FREE; 02651 if(count>=5000) { 02652 count=0; 02653 mode=32; 02654 } 02655 } else if(mode==32) { 02656 motor[TIRE_FR].pwm = 10; 02657 motor[TIRE_FR].dir = BRAKE; 02658 motor[TIRE_FL].pwm = 10; 02659 motor[TIRE_FL].dir = BRAKE; 02660 motor[TIRE_BR].pwm = 10; 02661 motor[TIRE_BR].dir = BRAKE; 02662 motor[TIRE_BL].pwm = 10; 02663 motor[TIRE_BL].dir = BRAKE; 02664 if(count>=40000) { 02665 mode=33; 02666 count=0; 02667 } 02668 } else if(mode==33) { 02669 motor[TIRE_FR].pwm = 0; 02670 motor[TIRE_FR].dir = BACK; 02671 motor[TIRE_FL].pwm = 30; 02672 motor[TIRE_FL].dir = FOR; 02673 motor[TIRE_BR].pwm = 30; 02674 motor[TIRE_BR].dir = BACK; 02675 motor[TIRE_BL].pwm = 0; 02676 motor[TIRE_BL].dir = FOR; 02677 if(g[3]==0&&count>=20000) { 02678 count=0; 02679 if(Twsh==2) { 02680 solenoid.solenoid2 = SOLENOID_OFF; 02681 } 02682 mode=40; 02683 } 02684 } else if(mode==40) { 02685 if(LIB) { 02686 mode=42; 02687 } 02688 if(counts==false&&g[0]==0) { 02689 cros++; 02690 counts=true; 02691 } 02692 if(counts==true&&g[0]==99) { 02693 counts=false; 02694 } 02695 02696 if(cros==1) { 02697 // mode=41; 02698 //count=0; 02699 //cros=0; 02700 }//ゆっくりモードに入れなかった時の保険 02701 02702 switch(g[3]) { 02703 case 98: 02704 motor[TIRE_FR].pwm = normal; 02705 motor[TIRE_FR].dir = BACK; 02706 motor[TIRE_FL].pwm = normal; 02707 motor[TIRE_FL].dir = BACK; 02708 motor[TIRE_BR].pwm = normal; 02709 motor[TIRE_BR].dir = FOR; 02710 motor[TIRE_BL].pwm = normal; 02711 motor[TIRE_BL].dir = FOR; 02712 if(count>20000) { 02713 cross++; 02714 count=0; 02715 } 02716 mtc2=true; 02717 if(cross==2) { 02718 mode=41; 02719 count=0; 02720 cross=0; 02721 } 02722 break; 02723 case 0: 02724 motor[TIRE_FR].pwm = fast; 02725 motor[TIRE_FR].dir = BACK; 02726 motor[TIRE_FL].pwm = fast; 02727 motor[TIRE_FL].dir = BACK; 02728 motor[TIRE_BR].pwm = fast; 02729 motor[TIRE_BR].dir = FOR; 02730 motor[TIRE_BL].pwm = fast; 02731 motor[TIRE_BL].dir = FOR; 02732 mtc2=true; 02733 break; 02734 case 255: 02735 motor[TIRE_FR].pwm = slow; 02736 motor[TIRE_FR].dir = BACK; 02737 motor[TIRE_FL].pwm = fast; 02738 motor[TIRE_FL].dir = BACK; 02739 motor[TIRE_BR].pwm = fast; 02740 motor[TIRE_BR].dir = FOR; 02741 motor[TIRE_BL].pwm = slow; 02742 motor[TIRE_BL].dir = FOR; 02743 mtc2=true; 02744 break; 02745 case 253: 02746 motor[TIRE_FR].pwm = 0; 02747 motor[TIRE_FR].dir = FREE; 02748 motor[TIRE_FL].pwm = slow; 02749 motor[TIRE_FL].dir = BACK; 02750 motor[TIRE_BR].pwm = slow; 02751 motor[TIRE_BR].dir = FOR; 02752 motor[TIRE_BL].pwm = 0; 02753 motor[TIRE_BL].dir = FREE; 02754 mtc2=true; 02755 break; 02756 case 254: 02757 motor[TIRE_FR].pwm = 0; 02758 motor[TIRE_FR].dir = FREE; 02759 motor[TIRE_FL].pwm = normal; 02760 motor[TIRE_FL].dir = BACK; 02761 motor[TIRE_BR].pwm = normal; 02762 motor[TIRE_BR].dir = FOR; 02763 motor[TIRE_BL].pwm = 0; 02764 motor[TIRE_BL].dir = FREE; 02765 mtc2=true; 02766 break; 02767 case 1: 02768 motor[TIRE_FR].pwm = fast; 02769 motor[TIRE_FR].dir = BACK; 02770 motor[TIRE_FL].pwm = slow; 02771 motor[TIRE_FL].dir = BACK; 02772 motor[TIRE_BR].pwm = slow; 02773 motor[TIRE_BR].dir = FOR; 02774 motor[TIRE_BL].pwm = fast; 02775 motor[TIRE_BL].dir = FOR; 02776 mtc2=true; 02777 break; 02778 case 3: 02779 motor[TIRE_FR].pwm = slow; 02780 motor[TIRE_FR].dir = BACK; 02781 motor[TIRE_FL].pwm = 0; 02782 motor[TIRE_FL].dir = FREE; 02783 motor[TIRE_BR].pwm = 0; 02784 motor[TIRE_BR].dir = FREE; 02785 motor[TIRE_BL].pwm = slow; 02786 motor[TIRE_BL].dir = FOR; 02787 mtc2=true; 02788 break; 02789 case 2: 02790 motor[TIRE_FR].pwm = normal; 02791 motor[TIRE_FR].dir = BACK; 02792 motor[TIRE_FL].pwm = 0; 02793 motor[TIRE_FL].dir = FREE; 02794 motor[TIRE_BR].pwm = 0; 02795 motor[TIRE_BR].dir = FREE; 02796 motor[TIRE_BL].pwm = normal; 02797 motor[TIRE_BL].dir = FOR; 02798 mtc2=true; 02799 break; 02800 } 02801 if(g[2]!=98&&mtc2==true) { 02802 switch(g[2]) { 02803 case 0: 02804 mtc2=false; 02805 break; 02806 case 255: 02807 motor[TIRE_FL].pwm += 5; 02808 mtc2=false; 02809 break; 02810 case 253: 02811 motor[TIRE_FL].pwm += 10; 02812 mtc2=false; 02813 break; 02814 case 254: 02815 motor[TIRE_FL].pwm += 20; 02816 motor[TIRE_BL].pwm = 0; 02817 mtc2=false; 02818 break; 02819 case 1: 02820 motor[TIRE_BL].pwm += 5; 02821 mtc2=false; 02822 break; 02823 case 3: 02824 motor[TIRE_BL].pwm += 10; 02825 mtc2=false; 02826 break; 02827 case 2: 02828 motor[TIRE_BL].pwm += 20; 02829 motor[TIRE_FL].pwm = 0; 02830 mtc2=false; 02831 break; 02832 } 02833 } 02834 } else if(mode==41) { 02835 if(LIF||LIB) { 02836 mode=42; 02837 } 02838 motor[TIRE_FR].pwm = 0; 02839 motor[TIRE_FR].dir = FREE; 02840 motor[TIRE_FL].pwm = 30; 02841 motor[TIRE_FL].dir = BACK; 02842 motor[TIRE_BR].pwm = 30; 02843 motor[TIRE_BR].dir = FOR; 02844 motor[TIRE_BL].pwm = 0; 02845 motor[TIRE_BL].dir = FREE; 02846 if(g[1]==0) { 02847 count=0; 02848 mode=70; 02849 normal=60; 02850 fast=70; 02851 slow=40; 02852 } 02853 } else if(mode==42) { 02854 motor[TIRE_FR].pwm = 20; 02855 motor[TIRE_FR].dir = FOR; 02856 motor[TIRE_FL].pwm = 0; 02857 motor[TIRE_FL].dir = FREE; 02858 motor[TIRE_BR].pwm = 0; 02859 motor[TIRE_BR].dir = FREE; 02860 motor[TIRE_BL].pwm = 20; 02861 motor[TIRE_BL].dir = BACK; 02862 if(g[1]==0) { 02863 count=0; 02864 mode=70; 02865 } 02866 } else if(mode==70) { 02867 switch(g[1]) { 02868 case 99: 02869 if(count>=100000) { 02870 mode=100; 02871 } 02872 break; 02873 case 98: 02874 motor[TIRE_FR].pwm = normal; 02875 motor[TIRE_FR].dir = FOR; 02876 motor[TIRE_FL].pwm = normal; 02877 motor[TIRE_FL].dir = BACK; 02878 motor[TIRE_BR].pwm = normal; 02879 motor[TIRE_BR].dir = FOR; 02880 motor[TIRE_BL].pwm = normal; 02881 motor[TIRE_BL].dir = BACK; 02882 break; 02883 case 0: 02884 motor[TIRE_FR].pwm = 80; 02885 motor[TIRE_FR].dir = FOR; 02886 motor[TIRE_FL].pwm = 80; 02887 motor[TIRE_FL].dir = BACK; 02888 motor[TIRE_BR].pwm = 80; 02889 motor[TIRE_BR].dir = FOR; 02890 motor[TIRE_BL].pwm = 80; 02891 motor[TIRE_BL].dir = BACK; 02892 mtc=true; 02893 break; 02894 case 255: 02895 motor[TIRE_FR].pwm = fast; 02896 motor[TIRE_FR].dir = FOR; 02897 motor[TIRE_FL].pwm = slow; 02898 motor[TIRE_FL].dir = BACK; 02899 motor[TIRE_BR].pwm = slow; 02900 motor[TIRE_BR].dir = FOR; 02901 motor[TIRE_BL].pwm = fast; 02902 motor[TIRE_BL].dir = BACK; 02903 mtc=true; 02904 break; 02905 case 253: 02906 motor[TIRE_FR].pwm = slow; 02907 motor[TIRE_FR].dir = FOR; 02908 motor[TIRE_FL].pwm = 0; 02909 motor[TIRE_FL].dir = FREE; 02910 motor[TIRE_BR].pwm = 0; 02911 motor[TIRE_BR].dir = FREE; 02912 motor[TIRE_BL].pwm = slow; 02913 motor[TIRE_BL].dir = BACK; 02914 mtc=true; 02915 break; 02916 case 254: 02917 motor[TIRE_FR].pwm = normal; 02918 motor[TIRE_FR].dir = FOR; 02919 motor[TIRE_FL].pwm = 0; 02920 motor[TIRE_FL].dir = FREE; 02921 motor[TIRE_BR].pwm = 0; 02922 motor[TIRE_BR].dir = FREE; 02923 motor[TIRE_BL].pwm = normal; 02924 motor[TIRE_BL].dir = BACK; 02925 mtc=true; 02926 break; 02927 case 1: 02928 motor[TIRE_FR].pwm = slow; 02929 motor[TIRE_FR].dir = FOR; 02930 motor[TIRE_FL].pwm = fast; 02931 motor[TIRE_FL].dir = BACK; 02932 motor[TIRE_BR].pwm = fast; 02933 motor[TIRE_BR].dir = FOR; 02934 motor[TIRE_BL].pwm = slow; 02935 motor[TIRE_BL].dir = BACK; 02936 mtc=true; 02937 break; 02938 case 3: 02939 motor[TIRE_FR].pwm = 0; 02940 motor[TIRE_FR].dir = FREE; 02941 motor[TIRE_FL].pwm = slow; 02942 motor[TIRE_FL].dir = BACK; 02943 motor[TIRE_BR].pwm = slow; 02944 motor[TIRE_BR].dir = FOR; 02945 motor[TIRE_BL].pwm = 0; 02946 motor[TIRE_BL].dir = FREE; 02947 mtc=true; 02948 break; 02949 case 2: 02950 motor[TIRE_FR].pwm = 0; 02951 motor[TIRE_FR].dir = FREE; 02952 motor[TIRE_FL].pwm = normal; 02953 motor[TIRE_FL].dir = BACK; 02954 motor[TIRE_BR].pwm = normal; 02955 motor[TIRE_BR].dir = FOR; 02956 motor[TIRE_BL].pwm = 0; 02957 motor[TIRE_BL].dir = FREE; 02958 mtc=true; 02959 break; 02960 } 02961 if(g[1]!=98&&mtc==true) { 02962 switch(g[0]) { 02963 case 0: 02964 mtc=false; 02965 break; 02966 case 255: 02967 motor[TIRE_FL].pwm += 5; 02968 mtc=false; 02969 break; 02970 case 253: 02971 motor[TIRE_FL].pwm += 10; 02972 mtc=false; 02973 break; 02974 case 254: 02975 motor[TIRE_FL].pwm += 20; 02976 motor[TIRE_FR].pwm = 0; 02977 mtc=false; 02978 break; 02979 case 1: 02980 motor[TIRE_FR].pwm += 5; 02981 mtc=false; 02982 break; 02983 case 3: 02984 motor[TIRE_FR].pwm += 10; 02985 mtc=false; 02986 break; 02987 case 2: 02988 motor[TIRE_FR].pwm += 20; 02989 motor[TIRE_FL].pwm = 0; 02990 mtc=false; 02991 break; 02992 } 02993 } 02994 } else if(mode==100) { 02995 motor[TIRE_FR].pwm = 20; 02996 motor[TIRE_FR].dir = FOR; 02997 motor[TIRE_FL].pwm = 20; 02998 motor[TIRE_FL].dir = BACK; 02999 motor[TIRE_BR].pwm = 23; 03000 motor[TIRE_BR].dir = FOR; 03001 motor[TIRE_BL].pwm = 20; 03002 motor[TIRE_BL].dir = BACK; 03003 } else { 03004 } 03005 } 03006 #endif 03007 03008 #if USE_PROCESS_NUM>7 03009 static void Process7() 03010 { 03011 03012 } 03013 #endif 03014 03015 #if USE_PROCESS_NUM>8 03016 static void Process8() 03017 { 03018 03019 } 03020 #endif 03021 03022 #if USE_PROCESS_NUM>9 03023 static void Process9() 03024 { 03025 03026 } 03027 #endif 03028 #endif 03029 #pragma endregion PROCESS 03030 03031 static void AllActuatorReset() 03032 { 03033 03034 #ifdef USE_SOLENOID 03035 solenoid.all = ALL_SOLENOID_OFF; 03036 #endif 03037 03038 #ifdef USE_MOTOR 03039 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 03040 motor[i].dir = FREE; 03041 motor[i].pwm = 0; 03042 } 03043 #endif 03044 } 03045 03046 /* 03047 void BuzzerTimer_func() 03048 { 03049 buzzer = !buzzer; 03050 //LED_DEBUG0 = !LED_DEBUG0; 03051 } 03052 */ 03053 void TapeLedEms_func() 03054 { 03055 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 03056 } 03057 03058 #pragma region USER-DEFINED-FUNCTIONS 03059 03060 03061 #pragma endregion
Generated on Tue Jul 12 2022 17:15:57 by
1.7.2