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