大季 矢花
/
2019_Master_9_2
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 00158 bool start_flag = true; 00159 00160 int mode =0; 00161 int cross=0;//十字数える用 00162 int cros=0; 00163 int count=100000;//wait代替え 00164 00165 ///*********PWM調整用*********/// 00166 int fast =60; 00167 int normal = 40; 00168 int slow = 20; 00169 00170 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00171 00172 #ifdef USE_SUBPROCESS 00173 #if USE_PROCESS_NUM>0 00174 static void Process0(void); 00175 #endif 00176 #if USE_PROCESS_NUM>1 00177 static void Process1(void); 00178 #endif 00179 #if USE_PROCESS_NUM>2 00180 static void Process2(void); 00181 #endif 00182 #if USE_PROCESS_NUM>3 00183 static void Process3(void); 00184 #endif 00185 #if USE_PROCESS_NUM>4 00186 static void Process4(void); 00187 #endif 00188 #if USE_PROCESS_NUM>5 00189 static void Process5(void); 00190 #endif 00191 #if USE_PROCESS_NUM>6 00192 static void Process6(void); 00193 #endif 00194 #if USE_PROCESS_NUM>7 00195 static void Process7(void); 00196 #endif 00197 #if USE_PROCESS_NUM>8 00198 static void Process8(void); 00199 #endif 00200 #if USE_PROCESS_NUM>9 00201 static void Process9(void); 00202 #endif 00203 #endif 00204 00205 void SystemProcessInitialize() 00206 { 00207 #pragma region USER-DEFINED_VARIABLE_INIT 00208 /*Replace here with the initialization code of your variables.*/ 00209 #pragma endregion USER-DEFINED_VARIABLE_INIT 00210 00211 lock = true; 00212 processChangeComp = true; 00213 current = DEFAULT_PROCESS; 00214 00215 #ifdef USE_SUBPROCESS 00216 #if USE_PROCESS_NUM>0 00217 Process[0] = Process0; 00218 #endif 00219 #if USE_PROCESS_NUM>1 00220 Process[1] = Process1; 00221 #endif 00222 #if USE_PROCESS_NUM>2 00223 Process[2] = Process2; 00224 #endif 00225 #if USE_PROCESS_NUM>3 00226 Process[3] = Process3; 00227 #endif 00228 #if USE_PROCESS_NUM>4 00229 Process[4] = Process4; 00230 #endif 00231 #if USE_PROCESS_NUM>5 00232 Process[5] = Process5; 00233 #endif 00234 #if USE_PROCESS_NUM>6 00235 Process[6] = Process6; 00236 #endif 00237 #if USE_PROCESS_NUM>7 00238 Process[7] = Process7; 00239 #endif 00240 #if USE_PROCESS_NUM>8 00241 Process[8] = Process8; 00242 #endif 00243 #if USE_PROCESS_NUM>9 00244 Process[9] = Process9; 00245 #endif 00246 #endif 00247 } 00248 00249 static void SystemProcessUpdate() 00250 { 00251 #ifdef USE_SUBPROCESS 00252 if(controller->Button.HOME) lock = false; 00253 00254 if(controller->Button.START && processChangeComp) { 00255 current++; 00256 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00257 processChangeComp = false; 00258 } else if(controller->Button.SELECT && processChangeComp) { 00259 current--; 00260 if (current < 0) current = 0; 00261 processChangeComp = false; 00262 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00263 #endif 00264 00265 #ifdef USE_MOTOR 00266 ACTUATORHUB::MOTOR::Motor::Update(motor); 00267 #endif 00268 00269 #ifdef USE_SOLENOID 00270 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00271 #endif 00272 00273 #ifdef USE_RS485 00274 ACTUATORHUB::ActuatorHub::Update(); 00275 #endif 00276 00277 } 00278 00279 int g[8]; 00280 00281 void SystemProcess() 00282 { 00283 SystemProcessInitialize(); 00284 00285 while(1) { 00286 if(EMC==0) { 00287 fast=40; 00288 normal=30; 00289 slow=20; 00290 cross=0;//十字数える用 00291 cros=0; 00292 count=100000;//wait代替え 00293 UP_flag = false; 00294 SW_flag = false; 00295 Air_flag = false; 00296 zyouge=false; 00297 zyougedo=false; 00298 dz1=true; 00299 dz1i=false; 00300 dz2=true; 00301 dz2i=false; 00302 dz3=true; 00303 dz3i=false; 00304 dz4=true; 00305 dz4i=false; 00306 through=false; 00307 counts=false; 00308 mtc=false; 00309 mtc2 = false; 00310 current = 0; 00311 SystemProcessInitialize(); 00312 if(emcs==false) { 00313 AllActuatorReset(); 00314 emcs=true; 00315 } 00316 } else { 00317 emcs=false; 00318 } 00319 00320 if(mode==0) { 00321 if(ZoneSW) { 00322 LedMode(1); 00323 } else { 00324 LedMode(2); 00325 } 00326 } 00327 for(int i = 0; i < 8; i++) { 00328 g[i] = LineHub::GetPara(i); 00329 } 00330 if(ThSW) { 00331 Twsh=1; 00332 } else { 00333 Twsh=2; 00334 } 00335 00336 if(StertSW && start_flag) { 00337 start_flag = false; 00338 lock = false; 00339 if(ZoneSW) { 00340 mode=1; 00341 current = 2; 00342 } else { 00343 mode=1; 00344 current = 4; 00345 } 00346 } 00347 if(DWSW||DOSW) { 00348 if(DWSW) { 00349 zyouge=true; 00350 } 00351 if(DOSW) { 00352 if(DOLS) { 00353 motor[MOTOR_0].pwm = 100; 00354 motor[MOTOR_0].dir = BRAKE; 00355 } else { 00356 motor[MOTOR_0].pwm = 50; 00357 motor[MOTOR_0].dir = BACK; 00358 } 00359 } 00360 } else if(zyouge==false) { 00361 motor[MOTOR_0].pwm = 100; 00362 motor[MOTOR_0].dir = BRAKE; 00363 } 00364 if(zyouge) { 00365 if(Twsh==1) { 00366 motor[MOTOR_0].pwm = 220; 00367 motor[MOTOR_0].dir = FOR; 00368 if(UPLS) { 00369 motor[MOTOR_0].pwm = 100; 00370 motor[MOTOR_0].dir = BRAKE; 00371 zyouge=false; 00372 } 00373 } 00374 if(Twsh==2) { 00375 motor[MOTOR_0].pwm = 230; 00376 motor[MOTOR_0].dir = FOR; 00377 if(UPLS2) { 00378 motor[MOTOR_0].pwm = 100; 00379 motor[MOTOR_0].dir = BRAKE; 00380 zyouge=false; 00381 } 00382 } 00383 } 00384 if(AIRSW) { 00385 if(SW_flag==false) { 00386 if(Air_flag==false) { 00387 solenoid.solenoid3 = SOLENOID_ON; 00388 Air_flag=true; 00389 SW_flag=true; 00390 } else { 00391 solenoid.solenoid3 = SOLENOID_OFF; 00392 Air_flag=false; 00393 SW_flag=true; 00394 } 00395 } 00396 } else { 00397 if( SW_flag==true) { 00398 count++; 00399 if(count>=2000) { 00400 SW_flag=false; 00401 count=0; 00402 } 00403 } 00404 } 00405 00406 //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]); 00407 00408 #ifdef USE_MU 00409 controller = CONTROLLER::Controller::GetData(); 00410 #endif 00411 00412 #ifdef USE_ERRORCHECK 00413 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { //& start_flag 00414 CONTROLLER::Controller::DataReset(); 00415 AllActuatorReset(); 00416 lock = true; 00417 } else 00418 #endif 00419 { 00420 00421 #ifdef USE_SUBPROCESS 00422 if(!lock) { 00423 Process[current](); 00424 } else 00425 #endif 00426 { 00427 //ロック時の処理 00428 } 00429 } 00430 00431 SystemProcessUpdate(); 00432 } 00433 } 00434 00435 #pragma region PROCESS 00436 #ifdef USE_SUBPROCESS 00437 #if USE_PROCESS_NUM>0 00438 static void Process0() 00439 { 00440 /* 00441 mode=1; 00442 LedMode(1); 00443 if(ThSW) { 00444 Twsh=1; 00445 } else { 00446 Twsh=2; 00447 } 00448 00449 if(StertSW) { 00450 //start_flag == false; 00451 //lock = false; 00452 if(ZoneSW) { 00453 mode=1; 00454 current = 2; 00455 } else { 00456 mode=1; 00457 current = 5; 00458 } 00459 } 00460 if(DWSW) { 00461 zyouge=true; 00462 } 00463 if(DOSW) { 00464 zyougedo=true; 00465 } 00466 if(AIRSW) { 00467 if(SW_flag==false) { 00468 if(Air_flag==false) { 00469 solenoid.solenoid3 = SOLENOID_ON; 00470 Air_flag=true; 00471 SW_flag=true; 00472 } else { 00473 solenoid.solenoid3 = SOLENOID_OFF; 00474 Air_flag=false; 00475 SW_flag=true; 00476 } 00477 } 00478 } else { 00479 SW_flag=false; 00480 } 00481 00482 if(zyouge) { 00483 if(Twsh==1) { 00484 motor[MOTOR_0].pwm = 220; 00485 motor[MOTOR_0].dir = FOR; 00486 if(UPLS) { 00487 motor[MOTOR_0].pwm = 100; 00488 motor[MOTOR_0].dir = BRAKE; 00489 zyouge=false; 00490 } 00491 } 00492 if(Twsh==2) { 00493 motor[MOTOR_0].pwm = 230; 00494 motor[MOTOR_0].dir = FOR; 00495 if(UPLS2) { 00496 motor[MOTOR_0].pwm = 100; 00497 motor[MOTOR_0].dir = BRAKE; 00498 zyouge=false; 00499 } 00500 } 00501 } 00502 if(zyougedo) { 00503 motor[MOTOR_0].pwm = 50; 00504 motor[MOTOR_0].dir = BACK; 00505 if(DOLS) { 00506 motor[MOTOR_0].pwm = 100; 00507 motor[MOTOR_0].dir = BRAKE; 00508 zyougedo=false; 00509 } 00510 } 00511 */ 00512 } 00513 #endif 00514 00515 #if USE_PROCESS_NUM>1 00516 static void Process1() 00517 { 00518 LedMode(6); 00519 start_flag = true; 00520 mtc=false; 00521 mtc2 = false; 00522 mode=1; 00523 cross=0; 00524 Twsh=1; 00525 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00526 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00527 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00528 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00529 00530 motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00531 motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00532 motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00533 motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00534 00535 if(controller->Button.UP||controller->Button.DOWN) { 00536 00537 if(controller->Button.UP) { 00538 motor[MOTOR_0].pwm =200; 00539 motor[MOTOR_0].dir = FOR; 00540 if(UPLS) { 00541 motor[MOTOR_0].pwm = 0; 00542 motor[MOTOR_0].dir = BRAKE; 00543 } 00544 } 00545 if(controller->Button.DOWN) { 00546 motor[MOTOR_0].pwm = 50; 00547 motor[MOTOR_0].dir = BACK; 00548 } 00549 } else { 00550 motor[MOTOR_0].pwm = 0; 00551 motor[MOTOR_0].dir = BRAKE; 00552 } 00553 00554 if(controller->Button.Y) { 00555 if(dz2==true) { 00556 if(dz2i==false) { 00557 solenoid.solenoid2 = SOLENOID_ON; 00558 dz2i=true; 00559 } else { 00560 solenoid.solenoid2 = SOLENOID_OFF; 00561 dz2i=false; 00562 } 00563 dz2=false; 00564 } 00565 } else { 00566 dz2=true; 00567 } 00568 00569 if(controller->Button.A) { 00570 if(dz3==true) { 00571 if(dz3i==false) { 00572 00573 dz3i=true; 00574 } else { 00575 solenoid.solenoid3 = SOLENOID_OFF; 00576 dz3i=false; 00577 } 00578 dz3=false; 00579 } 00580 } else { 00581 dz3=true; 00582 } 00583 00584 if(controller->Button.B) { 00585 if(dz4==true) { 00586 if(dz4i==false) { 00587 solenoid.solenoid4 = SOLENOID_ON; 00588 dz4i=true; 00589 } else { 00590 solenoid.solenoid4 = SOLENOID_OFF; 00591 dz4i=false; 00592 } 00593 dz4=false; 00594 } 00595 } else { 00596 dz4=true; 00597 } 00598 /* 00599 if(controller->Button.RIGHT){ 00600 motor[MOTOR_1].dir = FOR; 00601 motor[MOTOR_1].pwm = 60; 00602 if (ARM_1){ 00603 motor[MOTOR_1].dir = BRAKE; 00604 } 00605 } 00606 else if(controller->Button.LEFT){ 00607 motor[MOTOR_1].dir = BACK; 00608 motor[MOTOR_1].pwm = 60; 00609 if (ARM_0){ 00610 motor[MOTOR_1].dir = BRAKE; 00611 } 00612 } 00613 */ 00614 } 00615 #endif 00616 00617 #if USE_PROCESS_NUM>2 00618 static void Process2() 00619 { 00620 // 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]); 00621 LedMode(3); 00622 count++; 00623 if(mode==1) {//スタートゾーンから白線検知 00624 fast=40; 00625 normal=30; 00626 slow=20; 00627 motor[TIRE_FR].pwm = normal; 00628 motor[TIRE_FR].dir = BACK; 00629 motor[TIRE_FL].pwm = normal; 00630 motor[TIRE_FL].dir = FOR; 00631 motor[TIRE_BR].pwm = normal; 00632 motor[TIRE_BR].dir = BACK; 00633 motor[TIRE_BL].pwm = normal; 00634 motor[TIRE_BL].dir = FOR; 00635 if(g[2]==0) { 00636 count=100000; 00637 cross=0; 00638 mode=3; 00639 } 00640 } else if(mode==3) { //横ライントレースから縦ライントレースへ 00641 motor[TIRE_FR].pwm = 0; 00642 motor[TIRE_FR].dir = FREE; 00643 motor[TIRE_FL].pwm = 25; 00644 motor[TIRE_FL].dir = FOR; 00645 motor[TIRE_BR].pwm = 25; 00646 motor[TIRE_BR].dir = BACK; 00647 motor[TIRE_BL].pwm = 0; 00648 motor[TIRE_BL].dir = FREE; 00649 if(g[0]==0) { 00650 mode=10; 00651 count=100000; 00652 } 00653 } else if(mode==10) { //縦ライントレース 00654 00655 if(counts==false&&g[2]==0) { 00656 cros++; 00657 counts=true; 00658 } 00659 if(counts==true&&g[2]==99) { 00660 counts=false; 00661 } 00662 00663 if(cros==Twsh) { 00664 mode=11; 00665 count=0; 00666 cros=0; 00667 }//ゆっくりモードに入れなかった時の保険 00668 switch(g[0]) { 00669 case 98: 00670 motor[TIRE_FR].pwm = normal; 00671 motor[TIRE_FR].dir = BACK; 00672 motor[TIRE_FL].pwm = normal; 00673 motor[TIRE_FL].dir = FOR; 00674 motor[TIRE_BR].pwm = normal; 00675 motor[TIRE_BR].dir = BACK; 00676 motor[TIRE_BL].pwm = normal; 00677 motor[TIRE_BL].dir = FOR; 00678 mtc=true; 00679 if(count>80000) { 00680 cross++; 00681 count=0; 00682 } 00683 if(cross==Twsh) { 00684 mode=11; 00685 count=0; 00686 cross=0; 00687 } 00688 break; 00689 case 0: 00690 motor[TIRE_FR].pwm = fast; 00691 motor[TIRE_FR].dir = BACK; 00692 motor[TIRE_FL].pwm = fast; 00693 motor[TIRE_FL].dir = FOR; 00694 motor[TIRE_BR].pwm = fast; 00695 motor[TIRE_BR].dir = BACK; 00696 motor[TIRE_BL].pwm = fast; 00697 motor[TIRE_BL].dir = FOR; 00698 mtc=true; 00699 break; 00700 case 255: 00701 motor[TIRE_FR].pwm = fast; 00702 motor[TIRE_FR].dir = BACK; 00703 motor[TIRE_FL].pwm = slow; 00704 motor[TIRE_FL].dir = FOR; 00705 motor[TIRE_BR].pwm = slow; 00706 motor[TIRE_BR].dir = BACK; 00707 motor[TIRE_BL].pwm = fast; 00708 motor[TIRE_BL].dir = FOR; 00709 mtc=true; 00710 break; 00711 case 253: 00712 motor[TIRE_FR].pwm = slow; 00713 motor[TIRE_FR].dir = BACK; 00714 motor[TIRE_FL].pwm = 0; 00715 motor[TIRE_FL].dir = FREE; 00716 motor[TIRE_BR].pwm = 0; 00717 motor[TIRE_BR].dir = FREE; 00718 motor[TIRE_BL].pwm = slow; 00719 motor[TIRE_BL].dir = FOR; 00720 mtc=true; 00721 break; 00722 case 254: 00723 motor[TIRE_FR].pwm = normal; 00724 motor[TIRE_FR].dir = BACK; 00725 motor[TIRE_FL].pwm = 0; 00726 motor[TIRE_FL].dir = FREE; 00727 motor[TIRE_BR].pwm = 0; 00728 motor[TIRE_BR].dir = FREE; 00729 motor[TIRE_BL].pwm = normal; 00730 motor[TIRE_BL].dir = FOR; 00731 mtc=true; 00732 break; 00733 case 1: 00734 motor[TIRE_FR].pwm = slow; 00735 motor[TIRE_FR].dir = BACK; 00736 motor[TIRE_FL].pwm = fast; 00737 motor[TIRE_FL].dir = FOR; 00738 motor[TIRE_BR].pwm = fast; 00739 motor[TIRE_BR].dir = BACK; 00740 motor[TIRE_BL].pwm = slow; 00741 motor[TIRE_BL].dir = FOR; 00742 mtc=true; 00743 break; 00744 case 3: 00745 motor[TIRE_FR].pwm = 0; 00746 motor[TIRE_FR].dir = FREE; 00747 motor[TIRE_FL].pwm = slow; 00748 motor[TIRE_FL].dir = FOR; 00749 motor[TIRE_BR].pwm = slow; 00750 motor[TIRE_BR].dir = BACK; 00751 motor[TIRE_BL].pwm = 0; 00752 motor[TIRE_BL].dir = FREE; 00753 mtc=true; 00754 break; 00755 case 2: 00756 motor[TIRE_FR].pwm = 0; 00757 motor[TIRE_FR].dir = FREE; 00758 motor[TIRE_FL].pwm = normal; 00759 motor[TIRE_FL].dir = FOR; 00760 motor[TIRE_BR].pwm = normal; 00761 motor[TIRE_BR].dir = BACK; 00762 motor[TIRE_BL].pwm = 0; 00763 motor[TIRE_BL].dir = FREE; 00764 mtc=true; 00765 break; 00766 } 00767 if(g[0]!=98&&mtc==true) { 00768 switch(g[1]) { 00769 case 0: 00770 mtc=false; 00771 break; 00772 case 255: 00773 motor[TIRE_BR].pwm += 5; 00774 mtc=false; 00775 break; 00776 case 253: 00777 motor[TIRE_BR].pwm += 10; 00778 mtc=false; 00779 break; 00780 case 254: 00781 motor[TIRE_BR].pwm += 20; 00782 motor[TIRE_BL].pwm = 0; 00783 mtc=false; 00784 break; 00785 case 1: 00786 motor[TIRE_BL].pwm += 5; 00787 mtc=false; 00788 break; 00789 case 3: 00790 motor[TIRE_BL].pwm += 10; 00791 mtc=false; 00792 break; 00793 case 2: 00794 motor[TIRE_BL].pwm += 20; 00795 motor[TIRE_BR].pwm = 0; 00796 mtc=false; 00797 break; 00798 } 00799 } 00800 } else if(mode==11) { 00801 motor[TIRE_FR].pwm = 0; 00802 motor[TIRE_FR].dir = FREE; 00803 motor[TIRE_FL].pwm = normal; 00804 motor[TIRE_FL].dir = FOR; 00805 motor[TIRE_BR].pwm = normal; 00806 motor[TIRE_BR].dir = BACK; 00807 motor[TIRE_BL].pwm = 0; 00808 motor[TIRE_BL].dir = FREE; 00809 if(g[2]==0) { 00810 count=0; 00811 cross=0; 00812 mode=20; 00813 } 00814 } else if(mode==20) { 00815 00816 if(Twsh==2) { 00817 solenoid.solenoid2 = SOLENOID_ON; 00818 } 00819 if(counts==false&&g[0]==0) { 00820 cros++; 00821 counts=true; 00822 } 00823 if(counts==true&&g[0]==99) { 00824 counts=false; 00825 } 00826 00827 if(cros==2) { 00828 00829 // mode=21; 00830 // count=0; 00831 // cros=0; 00832 }//ゆっくりモードに入れなかった時の保険 00833 00834 switch(g[2]) { 00835 case 98: 00836 motor[TIRE_FR].pwm = normal; 00837 motor[TIRE_FR].dir = FOR; 00838 motor[TIRE_FL].pwm = normal; 00839 motor[TIRE_FL].dir = FOR; 00840 motor[TIRE_BR].pwm = normal; 00841 motor[TIRE_BR].dir = BACK; 00842 motor[TIRE_BL].pwm = normal; 00843 motor[TIRE_BL].dir = BACK; 00844 if(count>50000) { 00845 cross++; 00846 count=0; 00847 } 00848 mtc2=true; 00849 if(cross==2) { 00850 mode=22; 00851 count=0; 00852 cross=0; 00853 } 00854 break; 00855 case 0: 00856 motor[TIRE_FR].pwm = fast; 00857 motor[TIRE_FR].dir = FOR; 00858 motor[TIRE_FL].pwm = fast; 00859 motor[TIRE_FL].dir = FOR; 00860 motor[TIRE_BR].pwm = fast; 00861 motor[TIRE_BR].dir = BACK; 00862 motor[TIRE_BL].pwm = fast; 00863 motor[TIRE_BL].dir = BACK; 00864 mtc2=true; 00865 break; 00866 case 255: 00867 motor[TIRE_FR].pwm = normal; 00868 motor[TIRE_FR].dir = FOR; 00869 motor[TIRE_FL].pwm = fast; 00870 motor[TIRE_FL].dir = FOR; 00871 motor[TIRE_BR].pwm = fast; 00872 motor[TIRE_BR].dir = BACK; 00873 motor[TIRE_BL].pwm = normal; 00874 motor[TIRE_BL].dir = BACK; 00875 mtc2=true; 00876 break; 00877 case 253: 00878 motor[TIRE_FR].pwm = 0; 00879 motor[TIRE_FR].dir = FREE; 00880 motor[TIRE_FL].pwm = slow; 00881 motor[TIRE_FL].dir = FOR; 00882 motor[TIRE_BR].pwm = slow; 00883 motor[TIRE_BR].dir = BACK; 00884 motor[TIRE_BL].pwm = 0; 00885 motor[TIRE_BL].dir = FREE; 00886 mtc2=true; 00887 break; 00888 case 254: 00889 motor[TIRE_FR].pwm = 0; 00890 motor[TIRE_FR].dir = FREE; 00891 motor[TIRE_FL].pwm = normal; 00892 motor[TIRE_FL].dir = FOR; 00893 motor[TIRE_BR].pwm = normal; 00894 motor[TIRE_BR].dir = BACK; 00895 motor[TIRE_BL].pwm = 0; 00896 motor[TIRE_BL].dir = FREE; 00897 mtc2=true; 00898 break; 00899 case 1: 00900 motor[TIRE_FR].pwm = fast; 00901 motor[TIRE_FR].dir = FOR; 00902 motor[TIRE_FL].pwm = slow; 00903 motor[TIRE_FL].dir = FOR; 00904 motor[TIRE_BR].pwm = slow; 00905 motor[TIRE_BR].dir = BACK; 00906 motor[TIRE_BL].pwm = fast; 00907 motor[TIRE_BL].dir = BACK; 00908 mtc2=true; 00909 break; 00910 case 3: 00911 motor[TIRE_FR].pwm = slow; 00912 motor[TIRE_FR].dir = FOR; 00913 motor[TIRE_FL].pwm = 0; 00914 motor[TIRE_FL].dir = FREE; 00915 motor[TIRE_BR].pwm = 0; 00916 motor[TIRE_BR].dir = FREE; 00917 motor[TIRE_BL].pwm = slow; 00918 motor[TIRE_BL].dir = BACK; 00919 mtc2=true; 00920 break; 00921 case 2: 00922 motor[TIRE_FR].pwm = normal; 00923 motor[TIRE_FR].dir = FOR; 00924 motor[TIRE_FL].pwm = 0; 00925 motor[TIRE_FL].dir = FREE; 00926 motor[TIRE_BR].pwm = 0; 00927 motor[TIRE_BR].dir = FREE; 00928 motor[TIRE_BL].pwm = normal; 00929 motor[TIRE_BL].dir = BACK; 00930 mtc2=true; 00931 break; 00932 } 00933 if(g[2]!=98&&mtc2==true) { 00934 switch(g[3]) { 00935 case 0: 00936 mtc2=false; 00937 break; 00938 case 255: 00939 motor[TIRE_BL].pwm += 5; 00940 mtc2=false; 00941 break; 00942 case 253: 00943 motor[TIRE_BL].pwm += 10; 00944 mtc2=false; 00945 break; 00946 case 254: 00947 motor[TIRE_BL].pwm += 20; 00948 motor[TIRE_FL].pwm = 0; 00949 mtc2=false; 00950 break; 00951 case 1: 00952 motor[TIRE_FL].pwm += 5; 00953 mtc2=false; 00954 break; 00955 case 3: 00956 motor[TIRE_FL].pwm += 10; 00957 mtc2=false; 00958 break; 00959 case 2: 00960 motor[TIRE_FL].pwm += 20; 00961 motor[TIRE_BL].pwm = 0; 00962 mtc2=false; 00963 break; 00964 } 00965 } 00966 } else if(mode==21) { 00967 motor[TIRE_FR].pwm = 30; 00968 motor[TIRE_FR].dir = FOR; 00969 motor[TIRE_FL].pwm = 30; 00970 motor[TIRE_FL].dir = FOR; 00971 motor[TIRE_BR].pwm = 30; 00972 motor[TIRE_BR].dir = BACK; 00973 motor[TIRE_BL].pwm = 30; 00974 motor[TIRE_BL].dir = BACK; 00975 if(g[2]==98&&count>=10000) { 00976 mode=22; 00977 count=0; 00978 } 00979 } 00980 if(YELLOW) { 00981 if(mode==22) { 00982 motor[TIRE_FR].pwm = 10; 00983 motor[TIRE_FR].dir = BRAKE; 00984 motor[TIRE_FL].pwm = 10; 00985 motor[TIRE_FL].dir = BRAKE; 00986 motor[TIRE_BR].pwm = 10; 00987 motor[TIRE_BR].dir = BRAKE; 00988 motor[TIRE_BL].pwm = 10; 00989 motor[TIRE_BL].dir = BRAKE; 00990 if(count<=100000) { 00991 mode=23; 00992 } 00993 } else if(mode==23) { 00994 motor[TIRE_FR].pwm = 20; 00995 motor[TIRE_FR].dir = BACK; 00996 motor[TIRE_FL].pwm = 20; 00997 motor[TIRE_FL].dir = BACK; 00998 motor[TIRE_BR].pwm = 20; 00999 motor[TIRE_BR].dir = FOR; 01000 motor[TIRE_BL].pwm = 20; 01001 motor[TIRE_BL].dir = FOR; 01002 if(g[2]!=98) { 01003 count=0; 01004 mode=24; 01005 } 01006 } else if(mode==24) { 01007 motor[TIRE_FR].pwm = 10; 01008 motor[TIRE_FR].dir = BRAKE; 01009 motor[TIRE_FL].pwm = 10; 01010 motor[TIRE_FL].dir = BRAKE; 01011 motor[TIRE_BR].pwm = 10; 01012 motor[TIRE_BR].dir = BRAKE; 01013 motor[TIRE_BL].pwm = 10; 01014 motor[TIRE_BL].dir = BRAKE; 01015 if(count<=100000) { 01016 mode=25; 01017 } 01018 } else if(mode==25) { 01019 motor[TIRE_FR].pwm = 15; 01020 motor[TIRE_FR].dir = FOR; 01021 motor[TIRE_FL].pwm = 15; 01022 motor[TIRE_FL].dir = FOR; 01023 motor[TIRE_BR].pwm = 15; 01024 motor[TIRE_BR].dir = FOR; 01025 motor[TIRE_BL].pwm = 15; 01026 motor[TIRE_BL].dir = FOR; 01027 if(count<=100000) { 01028 mode=30; 01029 } 01030 } else if(mode==24) { 01031 motor[TIRE_FR].pwm = 10; 01032 motor[TIRE_FR].dir = BRAKE; 01033 motor[TIRE_FL].pwm = 10; 01034 motor[TIRE_FL].dir = BRAKE; 01035 motor[TIRE_BR].pwm = 10; 01036 motor[TIRE_BR].dir = BRAKE; 01037 motor[TIRE_BL].pwm = 10; 01038 motor[TIRE_BL].dir = BRAKE; 01039 if(count<=100000) { 01040 mode=30; 01041 } 01042 } 01043 } else { 01044 if(mode==22) { 01045 motor[TIRE_FR].pwm = 0; 01046 motor[TIRE_FR].dir = FREE; 01047 motor[TIRE_FL].pwm = 25; 01048 motor[TIRE_FL].dir = FOR; 01049 motor[TIRE_BR].pwm = 25; 01050 motor[TIRE_BR].dir = BACK; 01051 motor[TIRE_BL].pwm = 0; 01052 motor[TIRE_BL].dir = FREE; 01053 if(g[0]==0) { 01054 mode=30; 01055 cros=0; 01056 } 01057 } 01058 } 01059 if(mode==30) { 01060 motor[TIRE_FR].pwm = 60; 01061 motor[TIRE_FR].dir = BACK; 01062 motor[TIRE_FL].pwm = 60; 01063 motor[TIRE_FL].dir = FOR; 01064 motor[TIRE_BR].pwm = 60; 01065 motor[TIRE_BR].dir = BACK; 01066 motor[TIRE_BL].pwm = 60; 01067 motor[TIRE_BL].dir = FOR; 01068 if(g[0]==98) { 01069 mode=31; 01070 count=0; 01071 } 01072 } else if(mode==31) { 01073 motor[TIRE_FR].pwm = 0; 01074 motor[TIRE_FR].dir = FREE; 01075 motor[TIRE_FL].pwm = 0; 01076 motor[TIRE_FL].dir = FREE; 01077 motor[TIRE_BR].pwm = 0; 01078 motor[TIRE_BR].dir = FREE; 01079 motor[TIRE_BL].pwm = 0; 01080 motor[TIRE_BL].dir = FREE; 01081 if(count>=5000) { 01082 count=0; 01083 mode=32; 01084 } 01085 } else if(mode==32) { 01086 motor[TIRE_FR].pwm = 10; 01087 motor[TIRE_FR].dir = BRAKE; 01088 motor[TIRE_FL].pwm = 10; 01089 motor[TIRE_FL].dir = BRAKE; 01090 motor[TIRE_BR].pwm = 10; 01091 motor[TIRE_BR].dir = BRAKE; 01092 motor[TIRE_BL].pwm = 10; 01093 motor[TIRE_BL].dir = BRAKE; 01094 if(count>=40000) { 01095 mode=33; 01096 count=0; 01097 } 01098 } else if(mode==33) { 01099 motor[TIRE_FR].pwm = 30; 01100 motor[TIRE_FR].dir = BACK; 01101 motor[TIRE_FL].pwm = 0; 01102 motor[TIRE_FL].dir = BACK; 01103 motor[TIRE_BR].pwm = 0; 01104 motor[TIRE_BR].dir = FOR; 01105 motor[TIRE_BL].pwm = 30; 01106 motor[TIRE_BL].dir = FOR; 01107 if(g[3]==0) { 01108 count=0; 01109 if(Twsh==2) { 01110 solenoid.solenoid2 = SOLENOID_OFF; 01111 } 01112 mode=40; 01113 } 01114 } else if(mode==40) { 01115 if(LIB) { 01116 mode=42; 01117 } 01118 if(counts==false&&g[0]==0) { 01119 cros++; 01120 counts=true; 01121 } 01122 if(counts==true&&g[0]==99) { 01123 counts=false; 01124 } 01125 01126 if(cros==1) { 01127 // mode=41; 01128 //count=0; 01129 //cros=0; 01130 }//ゆっくりモードに入れなかった時の保険 01131 01132 switch(g[3]) { 01133 case 98: 01134 motor[TIRE_FR].pwm = normal; 01135 motor[TIRE_FR].dir = BACK; 01136 motor[TIRE_FL].pwm = normal; 01137 motor[TIRE_FL].dir = BACK; 01138 motor[TIRE_BR].pwm = normal; 01139 motor[TIRE_BR].dir = FOR; 01140 motor[TIRE_BL].pwm = normal; 01141 motor[TIRE_BL].dir = FOR; 01142 if(count>20000) { 01143 cross++; 01144 count=0; 01145 } 01146 mtc2=true; 01147 if(cross==1) { 01148 mode=41; 01149 count=0; 01150 cross=0; 01151 } 01152 break; 01153 case 0: 01154 motor[TIRE_FR].pwm = fast; 01155 motor[TIRE_FR].dir = BACK; 01156 motor[TIRE_FL].pwm = fast; 01157 motor[TIRE_FL].dir = BACK; 01158 motor[TIRE_BR].pwm = fast; 01159 motor[TIRE_BR].dir = FOR; 01160 motor[TIRE_BL].pwm = fast; 01161 motor[TIRE_BL].dir = FOR; 01162 mtc2=true; 01163 break; 01164 case 255: 01165 motor[TIRE_FR].pwm = slow; 01166 motor[TIRE_FR].dir = BACK; 01167 motor[TIRE_FL].pwm = fast; 01168 motor[TIRE_FL].dir = BACK; 01169 motor[TIRE_BR].pwm = fast; 01170 motor[TIRE_BR].dir = FOR; 01171 motor[TIRE_BL].pwm = slow; 01172 motor[TIRE_BL].dir = FOR; 01173 mtc2=true; 01174 break; 01175 case 253: 01176 motor[TIRE_FR].pwm = 0; 01177 motor[TIRE_FR].dir = FREE; 01178 motor[TIRE_FL].pwm = slow; 01179 motor[TIRE_FL].dir = BACK; 01180 motor[TIRE_BR].pwm = slow; 01181 motor[TIRE_BR].dir = FOR; 01182 motor[TIRE_BL].pwm = 0; 01183 motor[TIRE_BL].dir = FREE; 01184 mtc2=true; 01185 break; 01186 case 254: 01187 motor[TIRE_FR].pwm = 0; 01188 motor[TIRE_FR].dir = FREE; 01189 motor[TIRE_FL].pwm = normal; 01190 motor[TIRE_FL].dir = BACK; 01191 motor[TIRE_BR].pwm = normal; 01192 motor[TIRE_BR].dir = FOR; 01193 motor[TIRE_BL].pwm = 0; 01194 motor[TIRE_BL].dir = FREE; 01195 mtc2=true; 01196 break; 01197 case 1: 01198 motor[TIRE_FR].pwm = fast; 01199 motor[TIRE_FR].dir = BACK; 01200 motor[TIRE_FL].pwm = slow; 01201 motor[TIRE_FL].dir = BACK; 01202 motor[TIRE_BR].pwm = slow; 01203 motor[TIRE_BR].dir = FOR; 01204 motor[TIRE_BL].pwm = fast; 01205 motor[TIRE_BL].dir = FOR; 01206 mtc2=true; 01207 break; 01208 case 3: 01209 motor[TIRE_FR].pwm = slow; 01210 motor[TIRE_FR].dir = BACK; 01211 motor[TIRE_FL].pwm = 0; 01212 motor[TIRE_FL].dir = FREE; 01213 motor[TIRE_BR].pwm = 0; 01214 motor[TIRE_BR].dir = FREE; 01215 motor[TIRE_BL].pwm = slow; 01216 motor[TIRE_BL].dir = FOR; 01217 mtc2=true; 01218 break; 01219 case 2: 01220 motor[TIRE_FR].pwm = normal; 01221 motor[TIRE_FR].dir = BACK; 01222 motor[TIRE_FL].pwm = 0; 01223 motor[TIRE_FL].dir = FREE; 01224 motor[TIRE_BR].pwm = 0; 01225 motor[TIRE_BR].dir = FREE; 01226 motor[TIRE_BL].pwm = normal; 01227 motor[TIRE_BL].dir = FOR; 01228 mtc2=true; 01229 break; 01230 } 01231 if(g[2]!=98&&mtc2==true) { 01232 switch(g[2]) { 01233 case 0: 01234 mtc2=false; 01235 break; 01236 case 255: 01237 motor[TIRE_FL].pwm += 5; 01238 mtc2=false; 01239 break; 01240 case 253: 01241 motor[TIRE_FL].pwm += 10; 01242 mtc2=false; 01243 break; 01244 case 254: 01245 motor[TIRE_FL].pwm += 20; 01246 motor[TIRE_BL].pwm = 0; 01247 mtc2=false; 01248 break; 01249 case 1: 01250 motor[TIRE_BL].pwm += 5; 01251 mtc2=false; 01252 break; 01253 case 3: 01254 motor[TIRE_BL].pwm += 10; 01255 mtc2=false; 01256 break; 01257 case 2: 01258 motor[TIRE_BL].pwm += 20; 01259 motor[TIRE_FL].pwm = 0; 01260 mtc2=false; 01261 break; 01262 } 01263 } 01264 } else if(mode==41) { 01265 if(LIF||LIB) { 01266 mode=42; 01267 } 01268 motor[TIRE_FR].pwm = 0; 01269 motor[TIRE_FR].dir = FREE; 01270 motor[TIRE_FL].pwm = 30; 01271 motor[TIRE_FL].dir = BACK; 01272 motor[TIRE_BR].pwm = 30; 01273 motor[TIRE_BR].dir = FOR; 01274 motor[TIRE_BL].pwm = 0; 01275 motor[TIRE_BL].dir = FREE; 01276 if(g[1]==0) { 01277 count=0; 01278 mode=70; 01279 } 01280 } else if(mode==42) { 01281 motor[TIRE_FR].pwm = 20; 01282 motor[TIRE_FR].dir = FOR; 01283 motor[TIRE_FL].pwm = 0; 01284 motor[TIRE_FL].dir = FREE; 01285 motor[TIRE_BR].pwm = 0; 01286 motor[TIRE_BR].dir = FREE; 01287 motor[TIRE_BL].pwm = 20; 01288 motor[TIRE_BL].dir = BACK; 01289 if(g[1]==0) { 01290 count=0; 01291 mode=70; 01292 } 01293 } else if(mode==70) { 01294 switch(g[1]) { 01295 case 99: 01296 if(count>=100000) { 01297 mode=100; 01298 } 01299 break; 01300 case 98: 01301 motor[TIRE_FR].pwm = normal; 01302 motor[TIRE_FR].dir = FOR; 01303 motor[TIRE_FL].pwm = normal; 01304 motor[TIRE_FL].dir = BACK; 01305 motor[TIRE_BR].pwm = normal; 01306 motor[TIRE_BR].dir = FOR; 01307 motor[TIRE_BL].pwm = normal; 01308 motor[TIRE_BL].dir = BACK; 01309 break; 01310 case 0: 01311 motor[TIRE_FR].pwm = 80; 01312 motor[TIRE_FR].dir = FOR; 01313 motor[TIRE_FL].pwm = 80; 01314 motor[TIRE_FL].dir = BACK; 01315 motor[TIRE_BR].pwm = 80; 01316 motor[TIRE_BR].dir = FOR; 01317 motor[TIRE_BL].pwm = 80; 01318 motor[TIRE_BL].dir = BACK; 01319 mtc=true; 01320 break; 01321 case 255: 01322 motor[TIRE_FR].pwm = fast; 01323 motor[TIRE_FR].dir = FOR; 01324 motor[TIRE_FL].pwm = slow; 01325 motor[TIRE_FL].dir = BACK; 01326 motor[TIRE_BR].pwm = slow; 01327 motor[TIRE_BR].dir = FOR; 01328 motor[TIRE_BL].pwm = fast; 01329 motor[TIRE_BL].dir = BACK; 01330 mtc=true; 01331 break; 01332 case 253: 01333 motor[TIRE_FR].pwm = slow; 01334 motor[TIRE_FR].dir = FOR; 01335 motor[TIRE_FL].pwm = 0; 01336 motor[TIRE_FL].dir = FREE; 01337 motor[TIRE_BR].pwm = 0; 01338 motor[TIRE_BR].dir = FREE; 01339 motor[TIRE_BL].pwm = slow; 01340 motor[TIRE_BL].dir = BACK; 01341 mtc=true; 01342 break; 01343 case 254: 01344 motor[TIRE_FR].pwm = normal; 01345 motor[TIRE_FR].dir = FOR; 01346 motor[TIRE_FL].pwm = 0; 01347 motor[TIRE_FL].dir = FREE; 01348 motor[TIRE_BR].pwm = 0; 01349 motor[TIRE_BR].dir = FREE; 01350 motor[TIRE_BL].pwm = normal; 01351 motor[TIRE_BL].dir = BACK; 01352 mtc=true; 01353 break; 01354 case 1: 01355 motor[TIRE_FR].pwm = slow; 01356 motor[TIRE_FR].dir = FOR; 01357 motor[TIRE_FL].pwm = fast; 01358 motor[TIRE_FL].dir = BACK; 01359 motor[TIRE_BR].pwm = fast; 01360 motor[TIRE_BR].dir = FOR; 01361 motor[TIRE_BL].pwm = slow; 01362 motor[TIRE_BL].dir = BACK; 01363 mtc=true; 01364 break; 01365 case 3: 01366 motor[TIRE_FR].pwm = 0; 01367 motor[TIRE_FR].dir = FREE; 01368 motor[TIRE_FL].pwm = slow; 01369 motor[TIRE_FL].dir = BACK; 01370 motor[TIRE_BR].pwm = slow; 01371 motor[TIRE_BR].dir = FOR; 01372 motor[TIRE_BL].pwm = 0; 01373 motor[TIRE_BL].dir = FREE; 01374 mtc=true; 01375 break; 01376 case 2: 01377 motor[TIRE_FR].pwm = 0; 01378 motor[TIRE_FR].dir = FREE; 01379 motor[TIRE_FL].pwm = normal; 01380 motor[TIRE_FL].dir = BACK; 01381 motor[TIRE_BR].pwm = normal; 01382 motor[TIRE_BR].dir = FOR; 01383 motor[TIRE_BL].pwm = 0; 01384 motor[TIRE_BL].dir = FREE; 01385 mtc=true; 01386 break; 01387 } 01388 if(g[1]!=98&&mtc==true) { 01389 switch(g[0]) { 01390 case 0: 01391 mtc=false; 01392 break; 01393 case 255: 01394 motor[TIRE_FL].pwm += 5; 01395 mtc=false; 01396 break; 01397 case 253: 01398 motor[TIRE_FL].pwm += 10; 01399 mtc=false; 01400 break; 01401 case 254: 01402 motor[TIRE_FL].pwm += 20; 01403 motor[TIRE_FR].pwm = 0; 01404 mtc=false; 01405 break; 01406 case 1: 01407 motor[TIRE_FR].pwm += 5; 01408 mtc=false; 01409 break; 01410 case 3: 01411 motor[TIRE_FR].pwm += 10; 01412 mtc=false; 01413 break; 01414 case 2: 01415 motor[TIRE_FR].pwm += 20; 01416 motor[TIRE_FL].pwm = 0; 01417 mtc=false; 01418 break; 01419 } 01420 } 01421 } else if(mode==100) { 01422 motor[TIRE_FR].pwm = 20; 01423 motor[TIRE_FR].dir = FOR; 01424 motor[TIRE_FL].pwm = 20; 01425 motor[TIRE_FL].dir = BACK; 01426 motor[TIRE_BR].pwm = 23; 01427 motor[TIRE_BR].dir = FOR; 01428 motor[TIRE_BL].pwm = 20; 01429 motor[TIRE_BL].dir = BACK; 01430 } else { 01431 } 01432 } 01433 #endif 01434 01435 #if USE_PROCESS_NUM>3 01436 static void Process3() 01437 { 01438 } 01439 #endif 01440 01441 #if USE_PROCESS_NUM>4 01442 static void Process4() 01443 { 01444 LedMode(4); 01445 count++; 01446 if(mode==1) {//スタートゾーンから白線検知 01447 fast=50; 01448 normal=30; 01449 slow=20; 01450 motor[TIRE_FR].pwm = 35; 01451 motor[TIRE_FR].dir = FOR; 01452 motor[TIRE_FL].pwm = 35; 01453 motor[TIRE_FL].dir = BACK; 01454 motor[TIRE_BR].pwm = 35; 01455 motor[TIRE_BR].dir = FOR; 01456 motor[TIRE_BL].pwm = 35; 01457 motor[TIRE_BL].dir = BACK; 01458 if(g[2]==0) { 01459 count=100000; 01460 cross=0; 01461 mode=3; 01462 } 01463 } else if(mode==3) { //横ライントレースから縦ライントレースへ 01464 motor[TIRE_FR].pwm = 30; 01465 motor[TIRE_FR].dir = FOR; 01466 motor[TIRE_FL].pwm = 0; 01467 motor[TIRE_FL].dir = FREE; 01468 motor[TIRE_BR].pwm = 0; 01469 motor[TIRE_BR].dir = FREE; 01470 motor[TIRE_BL].pwm = 30; 01471 motor[TIRE_BL].dir = BACK; 01472 if(g[0]==0) { 01473 mode=10; 01474 count=100000; 01475 } 01476 } else if(mode==10) { //縦ライントレース 01477 01478 if(counts==false&&g[2]==0) { 01479 cros++; 01480 counts=true; 01481 } 01482 if(counts==true&&g[2]==99) { 01483 counts=false; 01484 } 01485 01486 if(cros==Twsh) { 01487 mode=11; 01488 count=0; 01489 cros=0; 01490 }//ゆっくりモードに入れなかった時の保険 01491 switch(g[1]) { 01492 case 98: 01493 motor[TIRE_FR].pwm = normal; 01494 motor[TIRE_FR].dir = FOR; 01495 motor[TIRE_FL].pwm = normal; 01496 motor[TIRE_FL].dir = BACK; 01497 motor[TIRE_BR].pwm = normal; 01498 motor[TIRE_BR].dir = FOR; 01499 motor[TIRE_BL].pwm = normal; 01500 motor[TIRE_BL].dir = BACK; 01501 mtc=true; 01502 if(count>80000) { 01503 cross++; 01504 count=0; 01505 } 01506 if(cross==Twsh) { 01507 mode=11; 01508 count=0; 01509 cross=0; 01510 } 01511 break; 01512 case 0: 01513 motor[TIRE_FR].pwm = fast; 01514 motor[TIRE_FR].dir = FOR; 01515 motor[TIRE_FL].pwm = fast; 01516 motor[TIRE_FL].dir = BACK; 01517 motor[TIRE_BR].pwm = fast; 01518 motor[TIRE_BR].dir = FOR; 01519 motor[TIRE_BL].pwm = fast; 01520 motor[TIRE_BL].dir = BACK; 01521 mtc=true; 01522 break; 01523 case 255: 01524 motor[TIRE_FR].pwm = fast; 01525 motor[TIRE_FR].dir = FOR; 01526 motor[TIRE_FL].pwm = slow; 01527 motor[TIRE_FL].dir = BACK; 01528 motor[TIRE_BR].pwm = slow; 01529 motor[TIRE_BR].dir = FOR; 01530 motor[TIRE_BL].pwm = fast; 01531 motor[TIRE_BL].dir = BACK; 01532 mtc=true; 01533 break; 01534 case 253: 01535 motor[TIRE_FR].pwm = slow; 01536 motor[TIRE_FR].dir = FOR; 01537 motor[TIRE_FL].pwm = 0; 01538 motor[TIRE_FL].dir = FREE; 01539 motor[TIRE_BR].pwm = 0; 01540 motor[TIRE_BR].dir = FREE; 01541 motor[TIRE_BL].pwm = slow; 01542 motor[TIRE_BL].dir = BACK; 01543 mtc=true; 01544 break; 01545 case 254: 01546 motor[TIRE_FR].pwm = normal; 01547 motor[TIRE_FR].dir = FOR; 01548 motor[TIRE_FL].pwm = 0; 01549 motor[TIRE_FL].dir = FREE; 01550 motor[TIRE_BR].pwm = 0; 01551 motor[TIRE_BR].dir = FREE; 01552 motor[TIRE_BL].pwm = normal; 01553 motor[TIRE_BL].dir = BACK; 01554 mtc=true; 01555 break; 01556 case 1: 01557 motor[TIRE_FR].pwm = slow; 01558 motor[TIRE_FR].dir = FOR; 01559 motor[TIRE_FL].pwm = fast; 01560 motor[TIRE_FL].dir = BACK; 01561 motor[TIRE_BR].pwm = fast; 01562 motor[TIRE_BR].dir = FOR; 01563 motor[TIRE_BL].pwm = slow; 01564 motor[TIRE_BL].dir = BACK; 01565 mtc=true; 01566 break; 01567 case 3: 01568 motor[TIRE_FR].pwm = 0; 01569 motor[TIRE_FR].dir = FREE; 01570 motor[TIRE_FL].pwm = slow; 01571 motor[TIRE_FL].dir = BACK; 01572 motor[TIRE_BR].pwm = slow; 01573 motor[TIRE_BR].dir = FOR; 01574 motor[TIRE_BL].pwm = 0; 01575 motor[TIRE_BL].dir = FREE; 01576 mtc=true; 01577 break; 01578 case 2: 01579 motor[TIRE_FR].pwm = 0; 01580 motor[TIRE_FR].dir = FREE; 01581 motor[TIRE_FL].pwm = normal; 01582 motor[TIRE_FL].dir = BACK; 01583 motor[TIRE_BR].pwm = normal; 01584 motor[TIRE_BR].dir = FOR; 01585 motor[TIRE_BL].pwm = 0; 01586 motor[TIRE_BL].dir = FREE; 01587 mtc=true; 01588 break; 01589 } 01590 if(g[0]!=98&&mtc==true) { 01591 switch(g[0]) { 01592 case 0: 01593 mtc=false; 01594 break; 01595 case 255: 01596 motor[TIRE_FL].pwm += 5; 01597 mtc=false; 01598 break; 01599 case 253: 01600 motor[TIRE_FL].pwm += 10; 01601 mtc=false; 01602 break; 01603 case 254: 01604 motor[TIRE_FL].pwm += 20; 01605 motor[TIRE_FR].pwm = 0; 01606 mtc=false; 01607 break; 01608 case 1: 01609 motor[TIRE_FR].pwm += 5; 01610 mtc=false; 01611 break; 01612 case 3: 01613 motor[TIRE_FR].pwm += 10; 01614 mtc=false; 01615 break; 01616 case 2: 01617 motor[TIRE_FR].pwm += 20; 01618 motor[TIRE_FL].pwm = 0; 01619 mtc=false; 01620 break; 01621 } 01622 } 01623 } else if(mode==11) { 01624 motor[TIRE_FR].pwm = normal; 01625 motor[TIRE_FR].dir = FOR; 01626 motor[TIRE_FL].pwm = 0; 01627 motor[TIRE_FL].dir = FREE; 01628 motor[TIRE_BR].pwm = 0; 01629 motor[TIRE_BR].dir = FREE; 01630 motor[TIRE_BL].pwm = normal; 01631 motor[TIRE_BL].dir = BACK; 01632 if(g[2]==0) { 01633 count=0; 01634 cross=0; 01635 mode=20; 01636 } 01637 } else if(mode==20) { 01638 01639 if(Twsh==2) { 01640 solenoid.solenoid2 = SOLENOID_ON; 01641 } 01642 01643 if(counts==false&&g[1]==0) { 01644 cros++; 01645 counts=true; 01646 } 01647 if(counts==true&&g[1]==99) { 01648 counts=false; 01649 } 01650 01651 if(cros==2) { 01652 //mode=21; 01653 //count=0; 01654 //cros=0; 01655 }//ゆっくりモードに入れなかった時の保険 01656 01657 switch(g[2]) { 01658 case 98: 01659 motor[TIRE_FR].pwm = normal; 01660 motor[TIRE_FR].dir = FOR; 01661 motor[TIRE_FL].pwm = normal; 01662 motor[TIRE_FL].dir = FOR; 01663 motor[TIRE_BR].pwm = normal; 01664 motor[TIRE_BR].dir = BACK; 01665 motor[TIRE_BL].pwm = normal; 01666 motor[TIRE_BL].dir = BACK; 01667 if(count>50000) { 01668 cross++; 01669 count=0; 01670 } 01671 mtc2=true; 01672 if(cross==1) { 01673 fast=30; 01674 normal=20; 01675 slow=20; 01676 } 01677 if(cross==2) { 01678 mode=21; 01679 cross=0; 01680 count=0; 01681 } 01682 break; 01683 case 0: 01684 motor[TIRE_FR].pwm = fast; 01685 motor[TIRE_FR].dir = FOR; 01686 motor[TIRE_FL].pwm = fast; 01687 motor[TIRE_FL].dir = FOR; 01688 motor[TIRE_BR].pwm = fast; 01689 motor[TIRE_BR].dir = BACK; 01690 motor[TIRE_BL].pwm = fast; 01691 motor[TIRE_BL].dir = BACK; 01692 mtc2=true; 01693 break; 01694 case 255: 01695 motor[TIRE_FR].pwm = normal; 01696 motor[TIRE_FR].dir = FOR; 01697 motor[TIRE_FL].pwm = fast; 01698 motor[TIRE_FL].dir = FOR; 01699 motor[TIRE_BR].pwm = fast; 01700 motor[TIRE_BR].dir = BACK; 01701 motor[TIRE_BL].pwm = normal; 01702 motor[TIRE_BL].dir = BACK; 01703 mtc2=true; 01704 break; 01705 case 253: 01706 motor[TIRE_FR].pwm = 0; 01707 motor[TIRE_FR].dir = FREE; 01708 motor[TIRE_FL].pwm = slow; 01709 motor[TIRE_FL].dir = FOR; 01710 motor[TIRE_BR].pwm = slow; 01711 motor[TIRE_BR].dir = BACK; 01712 motor[TIRE_BL].pwm = 0; 01713 motor[TIRE_BL].dir = FREE; 01714 mtc2=true; 01715 break; 01716 case 254: 01717 motor[TIRE_FR].pwm = 0; 01718 motor[TIRE_FR].dir = FREE; 01719 motor[TIRE_FL].pwm = normal; 01720 motor[TIRE_FL].dir = FOR; 01721 motor[TIRE_BR].pwm = normal; 01722 motor[TIRE_BR].dir = BACK; 01723 motor[TIRE_BL].pwm = 0; 01724 motor[TIRE_BL].dir = FREE; 01725 mtc2=true; 01726 break; 01727 case 1: 01728 motor[TIRE_FR].pwm = fast; 01729 motor[TIRE_FR].dir = FOR; 01730 motor[TIRE_FL].pwm = slow; 01731 motor[TIRE_FL].dir = FOR; 01732 motor[TIRE_BR].pwm = slow; 01733 motor[TIRE_BR].dir = BACK; 01734 motor[TIRE_BL].pwm = fast; 01735 motor[TIRE_BL].dir = BACK; 01736 mtc2=true; 01737 break; 01738 case 3: 01739 motor[TIRE_FR].pwm = slow; 01740 motor[TIRE_FR].dir = FOR; 01741 motor[TIRE_FL].pwm = 0; 01742 motor[TIRE_FL].dir = FREE; 01743 motor[TIRE_BR].pwm = 0; 01744 motor[TIRE_BR].dir = FREE; 01745 motor[TIRE_BL].pwm = slow; 01746 motor[TIRE_BL].dir = BACK; 01747 mtc2=true; 01748 break; 01749 case 2: 01750 motor[TIRE_FR].pwm = normal; 01751 motor[TIRE_FR].dir = FOR; 01752 motor[TIRE_FL].pwm = 0; 01753 motor[TIRE_FL].dir = FREE; 01754 motor[TIRE_BR].pwm = 0; 01755 motor[TIRE_BR].dir = FREE; 01756 motor[TIRE_BL].pwm = normal; 01757 motor[TIRE_BL].dir = BACK; 01758 mtc2=true; 01759 break; 01760 } 01761 if(g[2]!=98&&mtc2==true) { 01762 switch(g[3]) { 01763 case 0: 01764 mtc2=false; 01765 break; 01766 case 255: 01767 motor[TIRE_BL].pwm += 5; 01768 mtc2=false; 01769 break; 01770 case 253: 01771 motor[TIRE_BL].pwm += 10; 01772 mtc2=false; 01773 break; 01774 case 254: 01775 motor[TIRE_BL].pwm += 20; 01776 motor[TIRE_FL].pwm = 0; 01777 mtc2=false; 01778 break; 01779 case 1: 01780 motor[TIRE_FL].pwm += 5; 01781 mtc2=false; 01782 break; 01783 case 3: 01784 motor[TIRE_FL].pwm += 10; 01785 mtc2=false; 01786 break; 01787 case 2: 01788 motor[TIRE_FL].pwm += 20; 01789 motor[TIRE_BL].pwm = 0; 01790 mtc2=false; 01791 break; 01792 } 01793 } 01794 01795 } else if(mode==21) { 01796 motor[TIRE_FR].pwm = 20; 01797 motor[TIRE_FR].dir = BRAKE; 01798 motor[TIRE_FL].pwm = 20; 01799 motor[TIRE_FL].dir = BRAKE; 01800 motor[TIRE_BR].pwm = 20; 01801 motor[TIRE_BR].dir = BRAKE; 01802 motor[TIRE_BL].pwm = 20; 01803 motor[TIRE_BL].dir = BRAKE; 01804 if(count<=100000) { 01805 mode=22; 01806 } 01807 } 01808 if(YELLOW) { 01809 if(mode==22) { 01810 motor[TIRE_FR].pwm = 20; 01811 motor[TIRE_FR].dir = BACK; 01812 motor[TIRE_FL].pwm = 20; 01813 motor[TIRE_FL].dir = BACK; 01814 motor[TIRE_BR].pwm = 20; 01815 motor[TIRE_BR].dir = FOR; 01816 motor[TIRE_BL].pwm = 20; 01817 motor[TIRE_BL].dir = FOR; 01818 if(g[2]!=98) { 01819 mode=23; 01820 } 01821 } 01822 } else if(mode==23) { 01823 motor[TIRE_FR].pwm = 10; 01824 motor[TIRE_FR].dir = BRAKE; 01825 motor[TIRE_FL].pwm = 10; 01826 motor[TIRE_FL].dir = BRAKE; 01827 motor[TIRE_BR].pwm = 10; 01828 motor[TIRE_BR].dir = BRAKE; 01829 motor[TIRE_BL].pwm = 10; 01830 motor[TIRE_BL].dir = BRAKE; 01831 if(count<=100000) { 01832 mode=30; 01833 } 01834 } else { 01835 if(mode==22) { 01836 motor[TIRE_FR].pwm = normal; 01837 motor[TIRE_FR].dir = FOR; 01838 motor[TIRE_FL].pwm = 0; 01839 motor[TIRE_FL].dir = FREE; 01840 motor[TIRE_BR].pwm = 0; 01841 motor[TIRE_BR].dir = FREE; 01842 motor[TIRE_BL].pwm = normal; 01843 motor[TIRE_BL].dir = BACK; 01844 if(g[1]==0) { 01845 count=0; 01846 cross=0; 01847 mode=30; 01848 } 01849 } 01850 } 01851 if(mode==30) { 01852 motor[TIRE_FR].pwm = 60; 01853 motor[TIRE_FR].dir = FOR; 01854 motor[TIRE_FL].pwm = 60; 01855 motor[TIRE_FL].dir = BACK; 01856 motor[TIRE_BR].pwm = 60; 01857 motor[TIRE_BR].dir = FOR; 01858 motor[TIRE_BL].pwm = 60; 01859 motor[TIRE_BL].dir = BACK; 01860 if(g[1]==98) { 01861 mode=31; 01862 count=0; 01863 } 01864 } else if(mode==31) { 01865 motor[TIRE_FR].pwm = 0; 01866 motor[TIRE_FR].dir = FREE; 01867 motor[TIRE_FL].pwm = 0; 01868 motor[TIRE_FL].dir = FREE; 01869 motor[TIRE_BR].pwm = 0; 01870 motor[TIRE_BR].dir = FREE; 01871 motor[TIRE_BL].pwm = 0; 01872 motor[TIRE_BL].dir = FREE; 01873 if(count>=5000) { 01874 mode=32; 01875 count=0; 01876 } 01877 } else if(mode==32) { 01878 motor[TIRE_FR].pwm = 10; 01879 motor[TIRE_FR].dir = BRAKE; 01880 motor[TIRE_FL].pwm = 10; 01881 motor[TIRE_FL].dir = BRAKE; 01882 motor[TIRE_BR].pwm = 10; 01883 motor[TIRE_BR].dir = BRAKE; 01884 motor[TIRE_BL].pwm = 10; 01885 motor[TIRE_BL].dir = BRAKE; 01886 if(count>=30000) { 01887 mode=33; 01888 count=0; 01889 } 01890 } else if(mode==33) { 01891 motor[TIRE_FR].pwm = 0; 01892 motor[TIRE_FR].dir = FREE; 01893 motor[TIRE_FL].pwm = 25; 01894 motor[TIRE_FL].dir = BACK; 01895 motor[TIRE_BR].pwm = 25; 01896 motor[TIRE_BR].dir = FOR; 01897 motor[TIRE_BL].pwm = 0; 01898 motor[TIRE_BL].dir = FREE; 01899 if(g[3]==0) { 01900 count=0; 01901 if(Twsh==2) { 01902 solenoid.solenoid2 = SOLENOID_OFF; 01903 } 01904 mode=40; 01905 } 01906 } else if(mode==40) { 01907 if(LIF||LIB) { 01908 mode=42; 01909 } 01910 01911 if(counts==false&&g[0]==0) { 01912 cros++; 01913 counts=true; 01914 } 01915 if(counts==true&&g[0]==99) { 01916 counts=false; 01917 } 01918 01919 if(cros==2) { 01920 // mode=41; 01921 //count=0; 01922 //cros=0; 01923 }//ゆっくりモードに入れなかった時の保険 01924 01925 switch(g[3]) { 01926 case 98: 01927 motor[TIRE_FR].pwm = normal; 01928 motor[TIRE_FR].dir = BACK; 01929 motor[TIRE_FL].pwm = normal; 01930 motor[TIRE_FL].dir = BACK; 01931 motor[TIRE_BR].pwm = normal; 01932 motor[TIRE_BR].dir = FOR; 01933 motor[TIRE_BL].pwm = normal; 01934 motor[TIRE_BL].dir = FOR; 01935 if(count>20000) { 01936 cross++; 01937 count=0; 01938 } 01939 mtc2=true; 01940 if(cross==1) { 01941 mode=41; 01942 count=0; 01943 cross=0; 01944 } 01945 break; 01946 case 0: 01947 motor[TIRE_FR].pwm = fast; 01948 motor[TIRE_FR].dir = BACK; 01949 motor[TIRE_FL].pwm = fast; 01950 motor[TIRE_FL].dir = BACK; 01951 motor[TIRE_BR].pwm = fast; 01952 motor[TIRE_BR].dir = FOR; 01953 motor[TIRE_BL].pwm = fast; 01954 motor[TIRE_BL].dir = FOR; 01955 mtc2=true; 01956 break; 01957 case 255: 01958 motor[TIRE_FR].pwm = slow; 01959 motor[TIRE_FR].dir = BACK; 01960 motor[TIRE_FL].pwm = fast; 01961 motor[TIRE_FL].dir = BACK; 01962 motor[TIRE_BR].pwm = fast; 01963 motor[TIRE_BR].dir = FOR; 01964 motor[TIRE_BL].pwm = slow; 01965 motor[TIRE_BL].dir = FOR; 01966 mtc2=true; 01967 break; 01968 case 253: 01969 motor[TIRE_FR].pwm = 0; 01970 motor[TIRE_FR].dir = FREE; 01971 motor[TIRE_FL].pwm = slow; 01972 motor[TIRE_FL].dir = BACK; 01973 motor[TIRE_BR].pwm = slow; 01974 motor[TIRE_BR].dir = FOR; 01975 motor[TIRE_BL].pwm = 0; 01976 motor[TIRE_BL].dir = FREE; 01977 mtc2=true; 01978 break; 01979 case 254: 01980 motor[TIRE_FR].pwm = 0; 01981 motor[TIRE_FR].dir = FREE; 01982 motor[TIRE_FL].pwm = normal; 01983 motor[TIRE_FL].dir = BACK; 01984 motor[TIRE_BR].pwm = normal; 01985 motor[TIRE_BR].dir = FOR; 01986 motor[TIRE_BL].pwm = 0; 01987 motor[TIRE_BL].dir = FREE; 01988 mtc2=true; 01989 break; 01990 case 1: 01991 motor[TIRE_FR].pwm = fast; 01992 motor[TIRE_FR].dir = BACK; 01993 motor[TIRE_FL].pwm = slow; 01994 motor[TIRE_FL].dir = BACK; 01995 motor[TIRE_BR].pwm = slow; 01996 motor[TIRE_BR].dir = FOR; 01997 motor[TIRE_BL].pwm = fast; 01998 motor[TIRE_BL].dir = FOR; 01999 mtc2=true; 02000 break; 02001 case 3: 02002 motor[TIRE_FR].pwm = slow; 02003 motor[TIRE_FR].dir = BACK; 02004 motor[TIRE_FL].pwm = 0; 02005 motor[TIRE_FL].dir = FREE; 02006 motor[TIRE_BR].pwm = 0; 02007 motor[TIRE_BR].dir = FREE; 02008 motor[TIRE_BL].pwm = slow; 02009 motor[TIRE_BL].dir = FOR; 02010 mtc2=true; 02011 break; 02012 case 2: 02013 motor[TIRE_FR].pwm = normal; 02014 motor[TIRE_FR].dir = BACK; 02015 motor[TIRE_FL].pwm = 0; 02016 motor[TIRE_FL].dir = FREE; 02017 motor[TIRE_BR].pwm = 0; 02018 motor[TIRE_BR].dir = FREE; 02019 motor[TIRE_BL].pwm = normal; 02020 motor[TIRE_BL].dir = FOR; 02021 mtc2=true; 02022 break; 02023 } 02024 if(g[2]!=98&&mtc2==true) { 02025 switch(g[2]) { 02026 case 0: 02027 mtc2=false; 02028 break; 02029 case 255: 02030 motor[TIRE_FR].pwm += 5; 02031 mtc2=false; 02032 break; 02033 case 253: 02034 motor[TIRE_FR].pwm += 10; 02035 mtc2=false; 02036 break; 02037 case 254: 02038 motor[TIRE_FR].pwm += 20; 02039 motor[TIRE_BR].pwm = 0; 02040 mtc2=false; 02041 break; 02042 case 1: 02043 motor[TIRE_BR].pwm += 5; 02044 mtc2=false; 02045 break; 02046 case 3: 02047 motor[TIRE_BR].pwm += 10; 02048 mtc2=false; 02049 break; 02050 case 2: 02051 motor[TIRE_BR].pwm += 20; 02052 motor[TIRE_FR].pwm = 0; 02053 mtc2=false; 02054 break; 02055 } 02056 } 02057 } else if(mode==41) { 02058 if(LIF||LIB) { 02059 mode=42; 02060 } 02061 motor[TIRE_FR].pwm = 30; 02062 motor[TIRE_FR].dir = BACK; 02063 motor[TIRE_FL].pwm = 0; 02064 motor[TIRE_FL].dir = FREE; 02065 motor[TIRE_BR].pwm = 0; 02066 motor[TIRE_BR].dir = FREE; 02067 motor[TIRE_BL].pwm = 30; 02068 motor[TIRE_BL].dir = FOR; 02069 if(g[1]==0) { 02070 count=0; 02071 mode=70; 02072 normal=60; 02073 fast=80; 02074 slow=40; 02075 } 02076 } else if(mode==42) { 02077 motor[TIRE_FR].pwm = 0; 02078 motor[TIRE_FR].dir = FREE; 02079 motor[TIRE_FL].pwm = 30; 02080 motor[TIRE_FL].dir = FOR; 02081 motor[TIRE_BR].pwm = 30; 02082 motor[TIRE_BR].dir = BACK; 02083 motor[TIRE_BL].pwm = 0; 02084 motor[TIRE_BL].dir = FREE; 02085 if(g[0]==0) { 02086 count=0; 02087 mode=70; 02088 normal=60; 02089 fast=80; 02090 slow=40; 02091 } 02092 02093 } else if(mode==70) 02094 switch(g[0]) { 02095 case 99: 02096 if(count>=100000) { 02097 cross++; 02098 mode=100; 02099 count=0; 02100 } 02101 if(cross==2) { 02102 cross=0; 02103 mode=100; 02104 } 02105 break; 02106 case 98: 02107 motor[TIRE_FR].pwm = normal; 02108 motor[TIRE_FR].dir = BACK; 02109 motor[TIRE_FL].pwm = normal; 02110 motor[TIRE_FL].dir = FOR; 02111 motor[TIRE_BR].pwm = normal; 02112 motor[TIRE_BR].dir = BACK; 02113 motor[TIRE_BL].pwm = normal; 02114 motor[TIRE_BL].dir = FOR; 02115 break; 02116 case 0: 02117 motor[TIRE_FR].pwm = fast; 02118 motor[TIRE_FR].dir = BACK; 02119 motor[TIRE_FL].pwm = fast; 02120 motor[TIRE_FL].dir = FOR; 02121 motor[TIRE_BR].pwm = fast; 02122 motor[TIRE_BR].dir = BACK; 02123 motor[TIRE_BL].pwm = fast; 02124 motor[TIRE_BL].dir = FOR; 02125 mtc=true; 02126 break; 02127 case 255: 02128 motor[TIRE_FR].pwm = fast; 02129 motor[TIRE_FR].dir = BACK; 02130 motor[TIRE_FL].pwm = slow; 02131 motor[TIRE_FL].dir = FOR; 02132 motor[TIRE_BR].pwm = slow; 02133 motor[TIRE_BR].dir = BACK; 02134 motor[TIRE_BL].pwm = fast; 02135 motor[TIRE_BL].dir = FOR; 02136 mtc=true; 02137 break; 02138 case 253: 02139 motor[TIRE_FR].pwm = slow; 02140 motor[TIRE_FR].dir = BACK; 02141 motor[TIRE_FL].pwm = 0; 02142 motor[TIRE_FL].dir = FREE; 02143 motor[TIRE_BR].pwm = 0; 02144 motor[TIRE_BR].dir = FREE; 02145 motor[TIRE_BL].pwm = slow; 02146 motor[TIRE_BL].dir = FOR; 02147 mtc=true; 02148 break; 02149 case 254: 02150 motor[TIRE_FR].pwm = normal; 02151 motor[TIRE_FR].dir = BACK; 02152 motor[TIRE_FL].pwm = 0; 02153 motor[TIRE_FL].dir = FREE; 02154 motor[TIRE_BR].pwm = 0; 02155 motor[TIRE_BR].dir = FREE; 02156 motor[TIRE_BL].pwm = normal; 02157 motor[TIRE_BL].dir = FOR; 02158 mtc=true; 02159 break; 02160 case 1: 02161 motor[TIRE_FR].pwm = slow; 02162 motor[TIRE_FR].dir = BACK; 02163 motor[TIRE_FL].pwm = fast; 02164 motor[TIRE_FL].dir = FOR; 02165 motor[TIRE_BR].pwm = fast; 02166 motor[TIRE_BR].dir = BACK; 02167 motor[TIRE_BL].pwm = slow; 02168 motor[TIRE_BL].dir = FOR; 02169 mtc=true; 02170 break; 02171 case 3: 02172 motor[TIRE_FR].pwm = 0; 02173 motor[TIRE_FR].dir = FREE; 02174 motor[TIRE_FL].pwm = slow; 02175 motor[TIRE_FL].dir = FOR; 02176 motor[TIRE_BR].pwm = slow; 02177 motor[TIRE_BR].dir = BACK; 02178 motor[TIRE_BL].pwm = 0; 02179 motor[TIRE_BL].dir = FREE; 02180 mtc=true; 02181 break; 02182 case 2: 02183 motor[TIRE_FR].pwm = 0; 02184 motor[TIRE_FR].dir = FREE; 02185 motor[TIRE_FL].pwm = normal; 02186 motor[TIRE_FL].dir = FOR; 02187 motor[TIRE_BR].pwm = normal; 02188 motor[TIRE_BR].dir = BACK; 02189 motor[TIRE_BL].pwm = 0; 02190 motor[TIRE_BL].dir = FREE; 02191 mtc=true; 02192 break; 02193 } 02194 if(g[1]!=98&&mtc==true) { 02195 switch(g[1]) { 02196 case 0: 02197 mtc=false; 02198 break; 02199 case 255: 02200 motor[TIRE_BR].pwm += 5; 02201 mtc=false; 02202 break; 02203 case 253: 02204 motor[TIRE_BR].pwm += 10; 02205 mtc=false; 02206 break; 02207 case 254: 02208 motor[TIRE_BR].pwm += 20; 02209 motor[TIRE_BL].pwm = 0; 02210 mtc=false; 02211 break; 02212 case 1: 02213 motor[TIRE_BL].pwm += 5; 02214 mtc=false; 02215 break; 02216 case 3: 02217 motor[TIRE_BL].pwm += 10; 02218 mtc=false; 02219 break; 02220 case 2: 02221 motor[TIRE_BL].pwm += 20; 02222 motor[TIRE_BR].pwm = 0; 02223 mtc=false; 02224 break; 02225 } 02226 } else if(mode==100) { 02227 motor[TIRE_FR].pwm = 20; 02228 motor[TIRE_FR].dir = BACK; 02229 motor[TIRE_FL].pwm = 20; 02230 motor[TIRE_FL].dir = FOR; 02231 motor[TIRE_BR].pwm = 23; 02232 motor[TIRE_BR].dir = BACK; 02233 motor[TIRE_BL].pwm = 20; 02234 motor[TIRE_BL].dir = FOR; 02235 if(StertSW) { 02236 SW_flag = true; 02237 } 02238 } 02239 } 02240 #endif 02241 02242 #if USE_PROCESS_NUM>5 02243 static void Process5() 02244 { 02245 } 02246 #endif 02247 02248 #if USE_PROCESS_NUM>6 02249 static void Process6() 02250 { 02251 02252 } 02253 #endif 02254 02255 #if USE_PROCESS_NUM>7 02256 static void Process7() 02257 { 02258 02259 } 02260 #endif 02261 02262 #if USE_PROCESS_NUM>8 02263 static void Process8() 02264 { 02265 02266 } 02267 #endif 02268 02269 #if USE_PROCESS_NUM>9 02270 static void Process9() 02271 { 02272 02273 } 02274 #endif 02275 #endif 02276 #pragma endregion PROCESS 02277 02278 static void AllActuatorReset() 02279 { 02280 02281 #ifdef USE_SOLENOID 02282 solenoid.all = ALL_SOLENOID_OFF; 02283 #endif 02284 02285 #ifdef USE_MOTOR 02286 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 02287 motor[i].dir = FREE; 02288 motor[i].pwm = 0; 02289 } 02290 #endif 02291 } 02292 02293 /* 02294 void BuzzerTimer_func() 02295 { 02296 buzzer = !buzzer; 02297 //LED_DEBUG0 = !LED_DEBUG0; 02298 } 02299 */ 02300 void TapeLedEms_func() 02301 { 02302 sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; 02303 } 02304 02305 #pragma region USER-DEFINED-FUNCTIONS 02306 02307 02308 #pragma endregion
Generated on Thu Aug 11 2022 13:16:31 by
1.7.2