大季 矢花
/
MB2019_main_11_15_ok
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.
Embed:
(wiki syntax)
Show/hide line numbers
Process.cpp
00001 #include "mbed.h" 00002 #include "Process.h" 00003 #include <stdlib.h> 00004 #include "../../CommonLibraries/PID/PID.h" 00005 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" 00006 #include "../../Communication/RS485/LineHub/LineHub.h" 00007 #include "../../Communication/Controller/Controller.h" 00008 #include "../../Switch/Switch.h" 00009 00010 #include "../../LED/LED.h" 00011 #include "../../Safty/Safty.h" 00012 #include "../Using.h" 00013 00014 using namespace SWITCH; 00015 using namespace PID_SPACE; 00016 00017 using namespace LINEHUB; 00018 00019 static CONTROLLER::ControllerData *controller; 00020 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; 00021 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; 00022 00023 static bool lock; 00024 static bool processChangeComp; 00025 static int current; 00026 00027 static void AllActuatorReset(); 00028 00029 #ifdef USE_SUBPROCESS 00030 static void (*Process[USE_PROCESS_NUM])(void); 00031 #endif 00032 00033 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE 00034 00035 /*Replace here with the definition code of your variables.*/ 00036 00037 00038 //Serial pc(USBTX, USBRX); 00039 00040 const int omni[15][15] = { 00041 { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, 00042 { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, 00043 { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, 00044 { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, 00045 { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, 00046 { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, 00047 { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, 00048 { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, 00049 { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, 00050 { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, 00051 { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, 00052 { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, 00053 { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, 00054 { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, 00055 { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } 00056 }; 00057 00058 const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 }; 00059 00060 uint8_t SetStatus(int pwmVal); 00061 uint8_t SetPWM(int pwmVal); 00062 00063 DigitalIn EMC(PC_13); //非常停止確認用 00064 00065 bool start_flag = true; 00066 bool UP_flag = false; 00067 bool SW_flag = false; 00068 bool Air_flag = false; 00069 bool zyouge; 00070 bool zyougedo; 00071 bool dz2=true; 00072 bool dz2i=false; 00073 bool dz3=true; 00074 bool dz3i=false; 00075 bool dz4=true; 00076 bool dz4i=false; 00077 bool zone; 00078 bool through=false; 00079 bool counts=false; 00080 bool mtc=false; 00081 bool mtc2 = false; 00082 bool emcs=false; 00083 bool kiri; 00084 00085 int g[8]; 00086 int Twsh; 00087 int testmode =0; 00088 int mode =0; 00089 int cross=0;//十字数える用 00090 int cros=0; 00091 int countss=100000;//wait代替え 00092 00093 ///*********PWM調整用*********/// 00094 00095 int fast = 60; 00096 int normal = 40; 00097 int slow = 25; 00098 00099 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE 00100 00101 #ifdef USE_SUBPROCESS 00102 #if USE_PROCESS_NUM>0 00103 static void Process0(void); 00104 #endif 00105 #if USE_PROCESS_NUM>1 00106 static void Process1(void); 00107 #endif 00108 #if USE_PROCESS_NUM>2 00109 static void Process2(void); 00110 #endif 00111 #if USE_PROCESS_NUM>3 00112 static void Process3(void); 00113 #endif 00114 #if USE_PROCESS_NUM>4 00115 static void Process4(void); 00116 #endif 00117 #if USE_PROCESS_NUM>5 00118 static void Process5(void); 00119 #endif 00120 #if USE_PROCESS_NUM>6 00121 static void Process6(void); 00122 #endif 00123 #if USE_PROCESS_NUM>7 00124 static void Process7(void); 00125 #endif 00126 #if USE_PROCESS_NUM>8 00127 static void Process8(void); 00128 #endif 00129 #if USE_PROCESS_NUM>9 00130 static void Process9(void); 00131 #endif 00132 #endif 00133 00134 void SystemProcessInitialize() 00135 { 00136 #pragma region USER-DEFINED_VARIABLE_INIT 00137 /*Replace here with the initialization code of your variables.*/ 00138 00139 #pragma endregion USER-DEFINED_VARIABLE_INIT 00140 00141 lock = true; 00142 processChangeComp = true; 00143 current = DEFAULT_PROCESS; 00144 00145 #ifdef USE_SUBPROCESS 00146 #if USE_PROCESS_NUM>0 00147 Process[0] = Process0; 00148 #endif 00149 #if USE_PROCESS_NUM>1 00150 Process[1] = Process1; 00151 #endif 00152 #if USE_PROCESS_NUM>2 00153 Process[2] = Process2; 00154 #endif 00155 #if USE_PROCESS_NUM>3 00156 Process[3] = Process3; 00157 #endif 00158 #if USE_PROCESS_NUM>4 00159 Process[4] = Process4; 00160 #endif 00161 #if USE_PROCESS_NUM>5 00162 Process[5] = Process5; 00163 #endif 00164 #if USE_PROCESS_NUM>6 00165 Process[6] = Process6; 00166 #endif 00167 #if USE_PROCESS_NUM>7 00168 Process[7] = Process7; 00169 #endif 00170 #if USE_PROCESS_NUM>8 00171 Process[8] = Process8; 00172 #endif 00173 #if USE_PROCESS_NUM>9 00174 Process[9] = Process9; 00175 #endif 00176 #endif 00177 } 00178 00179 static void SystemProcessUpdate() 00180 { 00181 #ifdef USE_SUBPROCESS 00182 if(controller->Button.HOME) lock = false; 00183 00184 if(controller->Button.START && processChangeComp) { 00185 current++; 00186 if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; 00187 processChangeComp = false; 00188 } else if(controller->Button.SELECT && processChangeComp) { 00189 current--; 00190 if (current < 0) current = 0; 00191 processChangeComp = false; 00192 } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; 00193 #endif 00194 00195 #ifdef USE_MOTOR 00196 ACTUATORHUB::MOTOR::Motor::Update(motor); 00197 #endif 00198 00199 #ifdef USE_SOLENOID 00200 ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); 00201 #endif 00202 00203 #ifdef USE_RS485 00204 ACTUATORHUB::ActuatorHub::Update(); 00205 //LINEHUB::LineHub::Update(); 00206 #endif 00207 00208 } 00209 00210 int point1; 00211 void SystemProcess() 00212 { 00213 SystemProcessInitialize(); 00214 00215 while(1) { 00216 //pc.printf("3:%d\n\r",g[3]); 00217 00218 if(EMC==0) { 00219 fast=40; 00220 normal=30; 00221 slow=20; 00222 cross=0;//十字数える用 00223 cros=0; 00224 countss=0;//wait代替え 00225 UP_flag = false; 00226 SW_flag = false; 00227 Air_flag = false; 00228 zyouge=false; 00229 zyougedo=false; 00230 dz2=true; 00231 dz2i=false; 00232 dz3=true; 00233 dz3i=false; 00234 dz4=true; 00235 dz4i=false; 00236 through=false; 00237 counts=false; 00238 mtc=false; 00239 mtc2 = false; 00240 current = 0; 00241 SystemProcessInitialize(); 00242 if(emcs==false) { 00243 AllActuatorReset(); 00244 emcs=true; 00245 } 00246 } else { 00247 emcs=false; 00248 } 00249 00250 if(mode==0) { 00251 if(ZoneSW) { 00252 LED::LED::TapeledMode(1); 00253 } else { 00254 LED::LED::TapeledMode(2); 00255 } 00256 } 00257 for(int i = 0; i < 8; i++) { 00258 g[i] = LineHub::GetPara(i); 00259 } 00260 if(ThSW) { 00261 Twsh=1; 00262 } else { 00263 Twsh=2; 00264 } 00265 00266 if(StertSW && start_flag == true) { 00267 start_flag = false; 00268 lock = false; 00269 if(ZoneSW) { 00270 countss=0; 00271 mode=1; 00272 current = 3;//10GATU30変更した 00273 00274 } else { 00275 countss = 0; 00276 mode=1; 00277 current = 2;//11ガツ13ニチヘンコウ 00278 } 00279 } 00280 if(DWSW||DOSW) { 00281 if(DWSW) { 00282 zyouge=true; 00283 } 00284 if(DOSW) { 00285 if(DOLS) { 00286 motor[MOTOR_0].pwm = 100; 00287 motor[MOTOR_0].dir = BRAKE; 00288 } else { 00289 motor[MOTOR_0].pwm = 200; 00290 motor[MOTOR_0].dir = BACK; 00291 } 00292 } 00293 } else if(zyouge==false) { 00294 motor[MOTOR_0].pwm = 100; 00295 motor[MOTOR_0].dir = BRAKE; 00296 } 00297 if(zyouge) { 00298 if(Twsh==1) { 00299 motor[MOTOR_0].pwm = 255; 00300 motor[MOTOR_0].dir = FOR; 00301 if(UPLS) { 00302 motor[MOTOR_0].pwm = 100; 00303 motor[MOTOR_0].dir = BRAKE; 00304 zyouge=false; 00305 } 00306 } 00307 if(Twsh==2) { 00308 motor[MOTOR_0].pwm = 245; 00309 motor[MOTOR_0].dir = FOR; 00310 if(UPLS2) { 00311 motor[MOTOR_0].pwm = 100; 00312 motor[MOTOR_0].dir = BRAKE; 00313 zyouge=false; 00314 } 00315 } 00316 } 00317 if(AIRSW) { 00318 if(SW_flag==false) { 00319 if(Air_flag==false) { 00320 solenoid.solenoid3 = SOLENOID_ON; 00321 Air_flag=true; 00322 SW_flag=true; 00323 } else { 00324 solenoid.solenoid3 = SOLENOID_OFF; 00325 Air_flag=false; 00326 SW_flag=true; 00327 } 00328 } 00329 } else { 00330 if(SW_flag==true) { 00331 countss++; 00332 if(countss>=2000) { 00333 SW_flag=false; 00334 countss=0; 00335 } 00336 } 00337 } 00338 00339 00340 #ifdef USE_MU 00341 controller = CONTROLLER::Controller::GetData(); 00342 #endif 00343 00344 00345 #ifdef USE_ERRORCHECK 00346 if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { 00347 CONTROLLER::Controller::DataReset(); 00348 AllActuatorReset(); 00349 lock = true; 00350 } else 00351 00352 #endif 00353 { 00354 00355 #ifdef USE_SUBPROCESS 00356 if(!lock) { 00357 Process[current](); 00358 } else 00359 #endif 00360 { 00361 //ロック時の処理 00362 } 00363 } 00364 00365 SystemProcessUpdate(); 00366 } 00367 } 00368 00369 00370 00371 00372 #pragma region PROCESS 00373 #ifdef USE_SUBPROCESS 00374 #if USE_PROCESS_NUM>0 00375 static void Process0() 00376 { 00377 00378 //AllActuatorReset(); 00379 } 00380 #endif 00381 00382 #if USE_PROCESS_NUM>1 00383 static void Process1() 00384 { 00385 LED::LED::TapeledMode(6); 00386 start_flag = true; 00387 mtc=false; 00388 mtc2 = false; 00389 mode=1; 00390 cross=0; 00391 Twsh=1; 00392 motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); 00393 motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); 00394 motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00395 motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); 00396 00397 motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00398 motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2; 00399 motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00400 motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2; 00401 00402 if(controller->Button.UP||controller->Button.DOWN) { 00403 00404 if(controller->Button.UP) { 00405 motor[MOTOR_0].pwm =200; 00406 motor[MOTOR_0].dir = FOR; 00407 if(UPLS) { 00408 motor[MOTOR_0].pwm = 0; 00409 motor[MOTOR_0].dir = BRAKE; 00410 } 00411 } 00412 if(controller->Button.DOWN) { 00413 motor[MOTOR_0].pwm = 50; 00414 motor[MOTOR_0].dir = BACK; 00415 } 00416 } else { 00417 motor[MOTOR_0].pwm = 0; 00418 motor[MOTOR_0].dir = BRAKE; 00419 } 00420 00421 if(controller->Button.Y) { 00422 if(dz2==true) { 00423 if(dz2i==false) { 00424 solenoid.solenoid2 = SOLENOID_ON; 00425 dz2i=true; 00426 } else { 00427 solenoid.solenoid2 = SOLENOID_OFF; 00428 dz2i=false; 00429 } 00430 dz2=false; 00431 } 00432 } else { 00433 dz2=true; 00434 } 00435 00436 if(controller->Button.A) { 00437 if(dz3==true) { 00438 if(dz3i==false) { 00439 solenoid.solenoid3 = SOLENOID_ON; 00440 dz3i=true; 00441 } else { 00442 solenoid.solenoid3 = SOLENOID_OFF; 00443 dz3i=false; 00444 } 00445 dz3=false; 00446 } 00447 } else { 00448 dz3=true; 00449 } 00450 00451 if(controller->Button.B) { 00452 if(dz4==true) { 00453 if(dz4i==false) { 00454 solenoid.solenoid4 = SOLENOID_ON; 00455 dz4i=true; 00456 } else { 00457 solenoid.solenoid4 = SOLENOID_OFF; 00458 dz4i=false; 00459 } 00460 dz4=false; 00461 } 00462 } else { 00463 dz4=true; 00464 } 00465 } 00466 #endif 00467 00468 #if USE_PROCESS_NUM>2 00469 static void Process2() 00470 { 00471 //Oh So 青ゾーン 00472 LED::LED::TapeledMode(4); 00473 countss++; 00474 switch(mode) { 00475 case 1: 00476 fast=40; 00477 normal=30; 00478 slow=20; 00479 motor[TIRE_FR].pwm = normal+20; 00480 motor[TIRE_FR].dir = FOR; 00481 motor[TIRE_FL].pwm = normal+20; 00482 motor[TIRE_FL].dir = BACK; 00483 motor[TIRE_BR].pwm = normal+20; 00484 motor[TIRE_BR].dir = FOR; 00485 motor[TIRE_BL].pwm = normal+20; 00486 motor[TIRE_BL].dir = BACK; 00487 if(g[2]==0&&countss>=100000) { 00488 countss=100000; 00489 cross=0; 00490 mode=3; 00491 } 00492 break; 00493 case 3: 00494 motor[TIRE_FR].pwm = 30; 00495 motor[TIRE_FR].dir = FOR; 00496 motor[TIRE_FL].pwm = 255; 00497 motor[TIRE_FL].dir = BRAKE; 00498 motor[TIRE_BR].pwm = 255; 00499 motor[TIRE_BR].dir = BRAKE; 00500 motor[TIRE_BL].pwm = 30; 00501 motor[TIRE_BL].dir = BACK; 00502 if(g[1]==0) { 00503 mode=10; 00504 countss=100000; 00505 } 00506 break; 00507 case 10: 00508 switch(g[1]) { 00509 case 98: 00510 motor[TIRE_FR].pwm = normal+55; 00511 motor[TIRE_FR].dir = FOR; 00512 motor[TIRE_FL].pwm = normal+50; 00513 motor[TIRE_FL].dir = BACK; 00514 motor[TIRE_BR].pwm = normal+50; 00515 motor[TIRE_BR].dir = FOR; 00516 motor[TIRE_BL].pwm = normal+45; 00517 motor[TIRE_BL].dir = BACK; 00518 mtc=true; 00519 if(countss>80000) { 00520 cross++; 00521 countss=0; 00522 } 00523 00524 if(cross==2) { 00525 fast = 10; 00526 normal = 10; 00527 slow = 5; 00528 } 00529 00530 if(cross==3) { 00531 fast=40; 00532 normal=30; 00533 slow=20; 00534 mode=11; 00535 countss=0; 00536 cross=0; 00537 } 00538 break; 00539 case 0: 00540 motor[TIRE_FR].pwm = fast+55; 00541 motor[TIRE_FR].dir = FOR; 00542 motor[TIRE_FL].pwm = fast+50; 00543 motor[TIRE_FL].dir = BACK; 00544 motor[TIRE_BR].pwm = fast+50; 00545 motor[TIRE_BR].dir = FOR; 00546 motor[TIRE_BL].pwm = fast+45; 00547 motor[TIRE_BL].dir = BACK; 00548 mtc=true; 00549 break; 00550 case 255: 00551 motor[TIRE_FR].pwm = fast+55; 00552 motor[TIRE_FR].dir = FOR; 00553 motor[TIRE_FL].pwm = slow+50; 00554 motor[TIRE_FL].dir = BACK; 00555 motor[TIRE_BR].pwm = slow+50; 00556 motor[TIRE_BR].dir = FOR; 00557 motor[TIRE_BL].pwm = fast+45; 00558 motor[TIRE_BL].dir = BACK; 00559 mtc=true; 00560 break; 00561 case 253: 00562 motor[TIRE_FR].pwm = slow+55; 00563 motor[TIRE_FR].dir = FOR; 00564 motor[TIRE_FL].pwm = 0; 00565 motor[TIRE_FL].dir = FREE; 00566 motor[TIRE_BR].pwm = 0; 00567 motor[TIRE_BR].dir = FREE; 00568 motor[TIRE_BL].pwm = slow+45; 00569 motor[TIRE_BL].dir = BACK; 00570 mtc=true; 00571 break; 00572 case 254: 00573 motor[TIRE_FR].pwm = normal+55; 00574 motor[TIRE_FR].dir = FOR; 00575 motor[TIRE_FL].pwm = 0; 00576 motor[TIRE_FL].dir = FREE; 00577 motor[TIRE_BR].pwm = 0; 00578 motor[TIRE_BR].dir = FREE; 00579 motor[TIRE_BL].pwm = normal+45; 00580 motor[TIRE_BL].dir = BACK; 00581 mtc=true; 00582 break; 00583 case 1: 00584 motor[TIRE_FR].pwm = slow+55; 00585 motor[TIRE_FR].dir = FOR; 00586 motor[TIRE_FL].pwm = fast+50; 00587 motor[TIRE_FL].dir = BACK; 00588 motor[TIRE_BR].pwm = fast+50; 00589 motor[TIRE_BR].dir = FOR; 00590 motor[TIRE_BL].pwm = slow+45; 00591 motor[TIRE_BL].dir = BACK; 00592 mtc=true; 00593 break; 00594 case 3: 00595 motor[TIRE_FR].pwm = 0; 00596 motor[TIRE_FR].dir = FREE; 00597 motor[TIRE_FL].pwm = slow+50; 00598 motor[TIRE_FL].dir = BACK; 00599 motor[TIRE_BR].pwm = slow+50; 00600 motor[TIRE_BR].dir = FOR; 00601 motor[TIRE_BL].pwm = 0; 00602 motor[TIRE_BL].dir = FREE; 00603 mtc=true; 00604 break; 00605 case 2: 00606 motor[TIRE_FR].pwm = 0; 00607 motor[TIRE_FR].dir = FREE; 00608 motor[TIRE_FL].pwm = normal+50; 00609 motor[TIRE_FL].dir = BACK; 00610 motor[TIRE_BR].pwm = normal+50; 00611 motor[TIRE_BR].dir = FOR; 00612 motor[TIRE_BL].pwm = 0; 00613 motor[TIRE_BL].dir = FREE; 00614 mtc=true; 00615 break; 00616 } 00617 if(g[1]!=98&&mtc==true) { 00618 switch(g[0]) { 00619 case 0: 00620 mtc=false; 00621 break; 00622 case 255: 00623 motor[TIRE_FL].pwm += 3; 00624 mtc=false; 00625 break; 00626 case 253: 00627 motor[TIRE_FL].pwm += 6; 00628 mtc=false; 00629 break; 00630 case 254: 00631 motor[TIRE_FL].pwm += 9; 00632 motor[TIRE_FR].pwm = 0; 00633 mtc=false; 00634 break; 00635 case 1: 00636 motor[TIRE_FR].pwm += 3; 00637 mtc=false; 00638 break; 00639 case 3: 00640 motor[TIRE_FR].pwm += 6; 00641 mtc=false; 00642 break; 00643 case 2: 00644 motor[TIRE_FR].pwm += 9; 00645 motor[TIRE_FL].pwm = 0; 00646 mtc=false; 00647 break; 00648 } 00649 } 00650 break; 00651 case 11: 00652 motor[TIRE_FR].pwm = normal; 00653 motor[TIRE_FR].dir = FOR; 00654 motor[TIRE_FL].pwm = 15; 00655 motor[TIRE_FL].dir = FOR; 00656 motor[TIRE_BR].pwm = 15; 00657 motor[TIRE_BR].dir = BACK; 00658 motor[TIRE_BL].pwm = normal; 00659 motor[TIRE_BL].dir = BACK; 00660 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { 00661 countss=0; 00662 cross=0; 00663 mode=20; 00664 } 00665 break; 00666 case 20: 00667 solenoid.solenoid2= SOLENOID_ON; 00668 switch(g[2]) { 00669 case 98: 00670 motor[TIRE_FR].pwm = normal+20; 00671 motor[TIRE_FR].dir = FOR; 00672 motor[TIRE_FL].pwm = normal+20; 00673 motor[TIRE_FL].dir = FOR; 00674 motor[TIRE_BR].pwm = normal+20; 00675 motor[TIRE_BR].dir = BACK; 00676 motor[TIRE_BL].pwm = normal+20; 00677 motor[TIRE_BL].dir = BACK; 00678 if(countss>30000) { 00679 cross++; 00680 countss=0; 00681 } 00682 mtc2=true; 00683 if(YELLOW) { 00684 if(cross==1) { 00685 fast=20; 00686 normal=15; 00687 slow=15; 00688 } 00689 if(cross==2) { 00690 mode=21; 00691 countss=0; 00692 cross=0; 00693 } 00694 } else { 00695 if(cross==3) { 00696 mode=21; 00697 countss=0; 00698 cross=0; 00699 } 00700 } 00701 break; 00702 case 0: 00703 motor[TIRE_FR].pwm = fast+20; 00704 motor[TIRE_FR].dir = FOR; 00705 motor[TIRE_FL].pwm = fast+20; 00706 motor[TIRE_FL].dir = FOR; 00707 motor[TIRE_BR].pwm = fast+20; 00708 motor[TIRE_BR].dir = BACK; 00709 motor[TIRE_BL].pwm = fast+20; 00710 motor[TIRE_BL].dir = BACK; 00711 mtc2=true; 00712 break; 00713 case 255: 00714 motor[TIRE_FR].pwm = normal+20; 00715 motor[TIRE_FR].dir = FOR; 00716 motor[TIRE_FL].pwm = fast+20; 00717 motor[TIRE_FL].dir = FOR; 00718 motor[TIRE_BR].pwm = fast+20; 00719 motor[TIRE_BR].dir = BACK; 00720 motor[TIRE_BL].pwm = normal+20; 00721 motor[TIRE_BL].dir = BACK; 00722 mtc2=true; 00723 break; 00724 case 253: 00725 motor[TIRE_FR].pwm = 0; 00726 motor[TIRE_FR].dir = FREE; 00727 motor[TIRE_FL].pwm = slow+20; 00728 motor[TIRE_FL].dir = FOR; 00729 motor[TIRE_BR].pwm = slow+20; 00730 motor[TIRE_BR].dir = BACK; 00731 motor[TIRE_BL].pwm = 0; 00732 motor[TIRE_BL].dir = FREE; 00733 mtc2=true; 00734 break; 00735 case 254: 00736 motor[TIRE_FR].pwm = 0; 00737 motor[TIRE_FR].dir = FREE; 00738 motor[TIRE_FL].pwm = normal+20; 00739 motor[TIRE_FL].dir = FOR; 00740 motor[TIRE_BR].pwm = normal+20; 00741 motor[TIRE_BR].dir = BACK; 00742 motor[TIRE_BL].pwm = 0; 00743 motor[TIRE_BL].dir = FREE; 00744 mtc2=true; 00745 break; 00746 case 1: 00747 motor[TIRE_FR].pwm = fast+20; 00748 motor[TIRE_FR].dir = FOR; 00749 motor[TIRE_FL].pwm = slow+20; 00750 motor[TIRE_FL].dir = FOR; 00751 motor[TIRE_BR].pwm = slow+20; 00752 motor[TIRE_BR].dir = BACK; 00753 motor[TIRE_BL].pwm = fast+20; 00754 motor[TIRE_BL].dir = BACK; 00755 mtc2=true; 00756 break; 00757 case 3: 00758 motor[TIRE_FR].pwm = slow+20; 00759 motor[TIRE_FR].dir = FOR; 00760 motor[TIRE_FL].pwm = 0; 00761 motor[TIRE_FL].dir = FREE; 00762 motor[TIRE_BR].pwm = 0; 00763 motor[TIRE_BR].dir = FREE; 00764 motor[TIRE_BL].pwm = slow+20; 00765 motor[TIRE_BL].dir = BACK; 00766 mtc2=true; 00767 break; 00768 case 2: 00769 motor[TIRE_FR].pwm = normal+20; 00770 motor[TIRE_FR].dir = FOR; 00771 motor[TIRE_FL].pwm = 0; 00772 motor[TIRE_FL].dir = FREE; 00773 motor[TIRE_BR].pwm = 0; 00774 motor[TIRE_BR].dir = FREE; 00775 motor[TIRE_BL].pwm = normal+20; 00776 motor[TIRE_BL].dir = BACK; 00777 mtc2=true; 00778 break; 00779 } 00780 if(g[2]!=98&&mtc2==true) { 00781 switch(g[3]) { 00782 case 0: 00783 mtc2=false; 00784 break; 00785 case 255: 00786 motor[TIRE_BL].pwm += 5; 00787 mtc2=false; 00788 break; 00789 case 253: 00790 motor[TIRE_BL].pwm += 10; 00791 mtc2=false; 00792 break; 00793 case 254: 00794 motor[TIRE_BL].pwm += 15; 00795 motor[TIRE_FL].pwm = 0; 00796 mtc2=false; 00797 break; 00798 case 1: 00799 motor[TIRE_FL].pwm += 5; 00800 mtc2=false; 00801 break; 00802 case 3: 00803 motor[TIRE_FL].pwm += 10; 00804 mtc2=false; 00805 break; 00806 case 2: 00807 motor[TIRE_FL].pwm += 15; 00808 motor[TIRE_BL].pwm = 0; 00809 mtc2=false; 00810 break; 00811 } 00812 } 00813 break; 00814 case 21: 00815 if(YELLOW) { 00816 motor[TIRE_FR].pwm = 20; 00817 motor[TIRE_FR].dir = BACK; 00818 motor[TIRE_FL].pwm = 20; 00819 motor[TIRE_FL].dir = BACK; 00820 motor[TIRE_BR].pwm = 20; 00821 motor[TIRE_BR].dir = FOR; 00822 motor[TIRE_BL].pwm = 20; 00823 motor[TIRE_BL].dir = FOR; 00824 if(g[3]==98&&countss>=10000) { 00825 mode=22; 00826 countss=0; 00827 fast=60; 00828 normal=40; 00829 slow=20; 00830 } 00831 } else { 00832 motor[TIRE_FR].pwm = 255; 00833 motor[TIRE_FR].dir = BRAKE; 00834 motor[TIRE_FL].pwm = 30; 00835 motor[TIRE_FL].dir = FOR; 00836 motor[TIRE_BR].pwm = 30; 00837 motor[TIRE_BR].dir = BACK; 00838 motor[TIRE_BL].pwm = 255; 00839 motor[TIRE_BL].dir = BRAKE; 00840 if(g[0]==0) { 00841 mode=22; 00842 fast=60; 00843 normal=40; 00844 slow=25; 00845 } 00846 } 00847 break; 00848 case 22: 00849 if(YELLOW) { 00850 if(g[4]==3||g[4]==2) { 00851 motor[TIRE_FR].pwm = 30; 00852 motor[TIRE_FR].dir = FOR; 00853 motor[TIRE_FL].pwm = 30; 00854 motor[TIRE_FL].dir = FOR; 00855 motor[TIRE_BR].pwm = 30; 00856 motor[TIRE_BR].dir = BACK; 00857 motor[TIRE_BL].pwm = 30; 00858 motor[TIRE_BL].dir = BACK; 00859 } 00860 if(g[4]==253||g[4]==254) { 00861 motor[TIRE_FR].pwm = 30; 00862 motor[TIRE_FR].dir = BACK; 00863 motor[TIRE_FL].pwm = 30; 00864 motor[TIRE_FL].dir = BACK; 00865 motor[TIRE_BR].pwm = 30; 00866 motor[TIRE_BR].dir = FOR; 00867 motor[TIRE_BL].pwm = 30; 00868 motor[TIRE_BL].dir = FOR; 00869 } 00870 switch(g[2]) { 00871 case 0: 00872 motor[TIRE_FR].pwm = 20; 00873 motor[TIRE_FR].dir = FREE; 00874 motor[TIRE_BR].pwm = 20; 00875 motor[TIRE_BR].dir = FREE; 00876 break; 00877 case 255: 00878 motor[TIRE_FR].pwm = 30; 00879 motor[TIRE_FR].dir = BACK; 00880 motor[TIRE_BR].pwm = 20; 00881 motor[TIRE_BR].dir = FREE; 00882 break; 00883 case 253: 00884 motor[TIRE_FR].pwm = 40; 00885 motor[TIRE_FR].dir = BACK; 00886 motor[TIRE_BR].pwm = 20; 00887 motor[TIRE_BR].dir = FREE; 00888 break; 00889 case 254: 00890 motor[TIRE_FR].pwm = 30; 00891 motor[TIRE_FR].dir = BACK; 00892 motor[TIRE_BR].pwm = 30; 00893 motor[TIRE_BR].dir = BACK; 00894 break; 00895 case 1: 00896 motor[TIRE_FR].pwm = 20; 00897 motor[TIRE_FR].dir = FREE; 00898 motor[TIRE_BR].pwm = 30; 00899 motor[TIRE_BR].dir = FOR; 00900 break; 00901 case 3: 00902 motor[TIRE_FR].pwm = 20; 00903 motor[TIRE_FR].dir = FREE; 00904 motor[TIRE_BR].pwm = 40; 00905 motor[TIRE_BR].dir = FOR; 00906 break; 00907 case 2: 00908 motor[TIRE_FR].pwm = 30; 00909 motor[TIRE_FR].dir = FOR; 00910 motor[TIRE_BR].pwm = 30; 00911 motor[TIRE_BR].dir = FOR; 00912 break; 00913 } 00914 switch(g[3]) { 00915 case 0: 00916 motor[TIRE_FL].pwm = 25; 00917 motor[TIRE_FL].dir = FREE; 00918 motor[TIRE_BL].pwm = 20; 00919 motor[TIRE_BL].dir = FREE; 00920 break; 00921 case 255: 00922 motor[TIRE_FL].pwm = 25; 00923 motor[TIRE_FL].dir = FREE; 00924 motor[TIRE_BL].pwm = 30; 00925 motor[TIRE_BL].dir = BACK; 00926 case 253: 00927 motor[TIRE_FL].pwm = 25; 00928 motor[TIRE_FL].dir = FREE; 00929 motor[TIRE_BL].pwm = 40; 00930 motor[TIRE_BL].dir = BACK; 00931 break; 00932 case 254: 00933 motor[TIRE_FL].pwm = 35; 00934 motor[TIRE_FL].dir = BACK; 00935 motor[TIRE_BL].pwm = 30; 00936 motor[TIRE_BL].dir = BACK; 00937 break; 00938 case 1: 00939 motor[TIRE_FL].pwm = 35; 00940 motor[TIRE_FL].dir = FOR; 00941 motor[TIRE_BL].pwm = 20; 00942 motor[TIRE_BL].dir = FREE; 00943 break; 00944 case 3: 00945 motor[TIRE_FL].pwm = 45; 00946 motor[TIRE_FL].dir = FOR; 00947 motor[TIRE_BL].pwm = 20; 00948 motor[TIRE_BL].dir = FREE; 00949 break; 00950 case 2: 00951 motor[TIRE_FL].pwm = 35; 00952 motor[TIRE_FL].dir = FOR; 00953 motor[TIRE_BL].pwm = 30; 00954 motor[TIRE_BL].dir = FOR; 00955 break; 00956 } 00957 if(g[3]==0&&g[2]==0) { 00958 mode=23; 00959 countss=0; 00960 } 00961 } else { 00962 if(g[0]==98) { 00963 slow=25; 00964 normal=30; 00965 fast=30; 00966 } 00967 if(g[3]==0) { 00968 slow=25; 00969 normal=40; 00970 fast=60; 00971 mode=23; 00972 } 00973 switch(g[0]) { 00974 case 98: 00975 solenoid.solenoid2 = SOLENOID_OFF; 00976 fast=40; 00977 normal=30; 00978 slow=25; 00979 motor[TIRE_FR].pwm = normal; 00980 motor[TIRE_FR].dir = BACK; 00981 motor[TIRE_FL].pwm = normal+5; 00982 motor[TIRE_FL].dir = FOR; 00983 motor[TIRE_BR].pwm = normal; 00984 motor[TIRE_BR].dir = BACK; 00985 motor[TIRE_BL].pwm = normal; 00986 motor[TIRE_BL].dir = FOR; 00987 break; 00988 case 0: 00989 motor[TIRE_FR].pwm = fast; 00990 motor[TIRE_FR].dir = BACK; 00991 motor[TIRE_FL].pwm = fast+5; 00992 motor[TIRE_FL].dir = FOR; 00993 motor[TIRE_BR].pwm = fast; 00994 motor[TIRE_BR].dir = BACK; 00995 motor[TIRE_BL].pwm = fast; 00996 motor[TIRE_BL].dir = FOR; 00997 mtc=true; 00998 break; 00999 case 255: 01000 motor[TIRE_FR].pwm = fast; 01001 motor[TIRE_FR].dir = BACK; 01002 motor[TIRE_FL].pwm = slow+5; 01003 motor[TIRE_FL].dir = FOR; 01004 motor[TIRE_BR].pwm = slow; 01005 motor[TIRE_BR].dir = BACK; 01006 motor[TIRE_BL].pwm = fast; 01007 motor[TIRE_BL].dir = FOR; 01008 mtc=true; 01009 break; 01010 case 253: 01011 motor[TIRE_FR].pwm = slow; 01012 motor[TIRE_FR].dir = BACK; 01013 motor[TIRE_FL].pwm = 0; 01014 motor[TIRE_FL].dir = FREE; 01015 motor[TIRE_BR].pwm = 0; 01016 motor[TIRE_BR].dir = FREE; 01017 motor[TIRE_BL].pwm = slow; 01018 motor[TIRE_BL].dir = FOR; 01019 mtc=true; 01020 break; 01021 case 254: 01022 motor[TIRE_FR].pwm = normal; 01023 motor[TIRE_FR].dir = BACK; 01024 motor[TIRE_FL].pwm = 0; 01025 motor[TIRE_FL].dir = FREE; 01026 motor[TIRE_BR].pwm = 0; 01027 motor[TIRE_BR].dir = FREE; 01028 motor[TIRE_BL].pwm = normal; 01029 motor[TIRE_BL].dir = FOR; 01030 mtc=true; 01031 break; 01032 case 1: 01033 motor[TIRE_FR].pwm = slow; 01034 motor[TIRE_FR].dir = BACK; 01035 motor[TIRE_FL].pwm = fast+5; 01036 motor[TIRE_FL].dir = FOR; 01037 motor[TIRE_BR].pwm = fast; 01038 motor[TIRE_BR].dir = BACK; 01039 motor[TIRE_BL].pwm = slow; 01040 motor[TIRE_BL].dir = FOR; 01041 mtc=true; 01042 break; 01043 case 3: 01044 motor[TIRE_FR].pwm = 0; 01045 motor[TIRE_FR].dir = FREE; 01046 motor[TIRE_FL].pwm = slow+5; 01047 motor[TIRE_FL].dir = FOR; 01048 motor[TIRE_BR].pwm = slow; 01049 motor[TIRE_BR].dir = BACK; 01050 motor[TIRE_BL].pwm = 0; 01051 motor[TIRE_BL].dir = FREE; 01052 mtc=true; 01053 break; 01054 case 2: 01055 motor[TIRE_FR].pwm = 0; 01056 motor[TIRE_FR].dir = FREE; 01057 motor[TIRE_FL].pwm = normal+5; 01058 motor[TIRE_FL].dir = FOR; 01059 motor[TIRE_BR].pwm = normal; 01060 motor[TIRE_BR].dir = BACK; 01061 motor[TIRE_BL].pwm = 0; 01062 motor[TIRE_BL].dir = FREE; 01063 mtc=true; 01064 break; 01065 } 01066 if(g[0]!=98&&mtc==true) { 01067 switch(g[0]) { 01068 case 0: 01069 mtc=false; 01070 break; 01071 case 255: 01072 motor[TIRE_BR].pwm += 5; 01073 mtc=false; 01074 break; 01075 case 253: 01076 motor[TIRE_BR].pwm += 10; 01077 mtc=false; 01078 break; 01079 case 254: 01080 motor[TIRE_BR].pwm += 20; 01081 motor[TIRE_BL].pwm = 0; 01082 mtc=false; 01083 break; 01084 case 1: 01085 motor[TIRE_BL].pwm += 5; 01086 mtc=false; 01087 break; 01088 case 3: 01089 motor[TIRE_BL].pwm += 10; 01090 mtc=false; 01091 break; 01092 case 2: 01093 motor[TIRE_BL].pwm += 20; 01094 motor[TIRE_BR].pwm = 0; 01095 mtc=false; 01096 break; 01097 } 01098 } 01099 } 01100 break; 01101 case 23: 01102 if(YELLOW) { 01103 switch(g[4]) { 01104 case 98: 01105 motor[TIRE_FR].pwm = fast-5; 01106 motor[TIRE_FR].dir = BACK; 01107 motor[TIRE_FL].pwm = fast; 01108 motor[TIRE_FL].dir = FOR; 01109 motor[TIRE_BR].pwm = fast-5; 01110 motor[TIRE_BR].dir = BACK; 01111 motor[TIRE_BL].pwm = fast-5; 01112 motor[TIRE_BL].dir = FOR; 01113 break; 01114 case 99: 01115 motor[TIRE_FR].pwm = 0; 01116 motor[TIRE_FR].dir = FREE; 01117 motor[TIRE_FL].pwm = 0; 01118 motor[TIRE_FL].dir = FREE; 01119 motor[TIRE_BR].pwm = 0; 01120 motor[TIRE_BR].dir = FREE; 01121 motor[TIRE_BL].pwm = 0; 01122 motor[TIRE_BL].dir = FREE; 01123 break; 01124 case 0: 01125 motor[TIRE_FR].pwm = fast-5; 01126 motor[TIRE_FR].dir = BACK; 01127 motor[TIRE_FL].pwm = fast; 01128 motor[TIRE_FL].dir = FOR; 01129 motor[TIRE_BR].pwm = fast-5; 01130 motor[TIRE_BR].dir = BACK; 01131 motor[TIRE_BL].pwm = fast-5; 01132 motor[TIRE_BL].dir = FOR; 01133 break; 01134 case 255: 01135 motor[TIRE_FR].pwm = 0; 01136 motor[TIRE_FR].dir = FREE; 01137 motor[TIRE_FL].pwm = 0; 01138 motor[TIRE_FL].dir = FREE; 01139 motor[TIRE_BR].pwm = 0; 01140 motor[TIRE_BR].dir = FREE; 01141 motor[TIRE_BL].pwm = 0; 01142 motor[TIRE_BL].dir = FREE; 01143 break; 01144 case 253: 01145 motor[TIRE_FR].pwm = fast-5; 01146 motor[TIRE_FR].dir = BACK; 01147 motor[TIRE_FL].pwm = slow; 01148 motor[TIRE_FL].dir = FOR; 01149 motor[TIRE_BR].pwm = slow-5; 01150 motor[TIRE_BR].dir = BACK; 01151 motor[TIRE_BL].pwm = fast-5; 01152 motor[TIRE_BL].dir = FOR; 01153 break; 01154 case 254: 01155 motor[TIRE_FR].pwm = fast-5; 01156 motor[TIRE_FR].dir = BACK; 01157 motor[TIRE_FL].pwm = normal; 01158 motor[TIRE_FL].dir = FOR; 01159 motor[TIRE_BR].pwm = normal-5; 01160 motor[TIRE_BR].dir = BACK; 01161 motor[TIRE_BL].pwm = fast-5; 01162 motor[TIRE_BL].dir = FOR; 01163 break; 01164 case 1: 01165 motor[TIRE_FR].pwm = 0; 01166 motor[TIRE_FR].dir = FREE; 01167 motor[TIRE_FL].pwm = 0; 01168 motor[TIRE_FL].dir = FREE; 01169 motor[TIRE_BR].pwm = 0; 01170 motor[TIRE_BR].dir = FREE; 01171 motor[TIRE_BL].pwm = 0; 01172 motor[TIRE_BL].dir = FREE; 01173 break; 01174 case 3: 01175 motor[TIRE_FR].pwm = slow-5; 01176 motor[TIRE_FR].dir = BACK; 01177 motor[TIRE_FL].pwm = fast; 01178 motor[TIRE_FL].dir = FOR; 01179 motor[TIRE_BR].pwm = fast-5; 01180 motor[TIRE_BR].dir = BACK; 01181 motor[TIRE_BL].pwm = slow-5; 01182 motor[TIRE_BL].dir = FOR; 01183 break; 01184 case 2: 01185 motor[TIRE_FR].pwm = normal-5; 01186 motor[TIRE_FR].dir = BACK; 01187 motor[TIRE_FL].pwm = fast; 01188 motor[TIRE_FL].dir = FOR; 01189 motor[TIRE_BR].pwm = fast-5; 01190 motor[TIRE_BR].dir = BACK; 01191 motor[TIRE_BL].pwm = normal-5; 01192 motor[TIRE_BL].dir = FOR; 01193 break; 01194 } 01195 if(g[0]==98) { 01196 fast=20; 01197 normal=15; 01198 slow=10; 01199 } 01200 if(g[2]==254&&countss>=100000) { 01201 mode=24; 01202 solenoid.solenoid2 = SOLENOID_OFF; 01203 countss=0; 01204 fast=60; 01205 normal=40; 01206 slow=20; 01207 } 01208 } else { 01209 if(UPLS==true||DOLS==true) { 01210 if(UPLS==true||DOLS==true) { 01211 motor[MOTOR_0].pwm = 200; 01212 motor[MOTOR_0].dir = BRAKE; 01213 fast=60; 01214 normal=40; 01215 slow=30; 01216 countss=0; 01217 mode=24; 01218 } 01219 } else { 01220 motor[MOTOR_0].pwm = 100; 01221 motor[MOTOR_0].dir = BACK; 01222 } 01223 motor[TIRE_FR].pwm = 100; 01224 motor[TIRE_FR].dir = BRAKE; 01225 motor[TIRE_FL].pwm = 100; 01226 motor[TIRE_FL].dir = BRAKE; 01227 motor[TIRE_BR].pwm = 100; 01228 motor[TIRE_BR].dir = BRAKE; 01229 motor[TIRE_BL].pwm = 100; 01230 motor[TIRE_BL].dir = BRAKE; 01231 } 01232 break; 01233 case 24: 01234 if(YELLOW) { 01235 if(g[4]==3||g[4]==2) { 01236 motor[TIRE_FR].pwm = 30; 01237 motor[TIRE_FR].dir = FOR; 01238 motor[TIRE_FL].pwm = 30; 01239 motor[TIRE_FL].dir = FOR; 01240 motor[TIRE_BR].pwm = 30; 01241 motor[TIRE_BR].dir = BACK; 01242 motor[TIRE_BL].pwm = 30; 01243 motor[TIRE_BL].dir = BACK; 01244 } 01245 if(g[4]==253||g[4]==254) { 01246 motor[TIRE_FR].pwm = 30; 01247 motor[TIRE_FR].dir = BACK; 01248 motor[TIRE_FL].pwm = 30; 01249 motor[TIRE_FL].dir = BACK; 01250 motor[TIRE_BR].pwm = 30; 01251 motor[TIRE_BR].dir = FOR; 01252 motor[TIRE_BL].pwm = 30; 01253 motor[TIRE_BL].dir = FOR; 01254 } 01255 switch(g[2]) { 01256 case 0: 01257 motor[TIRE_FR].pwm = 25; 01258 motor[TIRE_FR].dir = FREE; 01259 motor[TIRE_BR].pwm = 25; 01260 motor[TIRE_BR].dir = FREE; 01261 break; 01262 case 255: 01263 motor[TIRE_FR].pwm = 30; 01264 motor[TIRE_FR].dir = BACK; 01265 motor[TIRE_BR].pwm = 25; 01266 motor[TIRE_BR].dir = FREE; 01267 break; 01268 case 253: 01269 motor[TIRE_FR].pwm = 40; 01270 motor[TIRE_FR].dir = BACK; 01271 motor[TIRE_BR].pwm = 25; 01272 motor[TIRE_BR].dir = FREE; 01273 break; 01274 case 254: 01275 motor[TIRE_FR].pwm = 30; 01276 motor[TIRE_FR].dir = BACK; 01277 motor[TIRE_BR].pwm = 30; 01278 motor[TIRE_BR].dir = BACK; 01279 break; 01280 case 1: 01281 motor[TIRE_FR].pwm = 25; 01282 motor[TIRE_FR].dir = FREE; 01283 motor[TIRE_BR].pwm = 30; 01284 motor[TIRE_BR].dir = FOR; 01285 break; 01286 case 3: 01287 motor[TIRE_FR].pwm = 25; 01288 motor[TIRE_FR].dir = FREE; 01289 motor[TIRE_BR].pwm = 40; 01290 motor[TIRE_BR].dir = FOR; 01291 break; 01292 case 2: 01293 motor[TIRE_FR].pwm = 30; 01294 motor[TIRE_FR].dir = FOR; 01295 motor[TIRE_BR].pwm = 30; 01296 motor[TIRE_BR].dir = FOR; 01297 break; 01298 } 01299 switch(g[3]) { 01300 case 0: 01301 motor[TIRE_FL].pwm = 25; 01302 motor[TIRE_FL].dir = FREE; 01303 motor[TIRE_BL].pwm = 25; 01304 motor[TIRE_BL].dir = FREE; 01305 break; 01306 case 255: 01307 motor[TIRE_FL].pwm = 25; 01308 motor[TIRE_FL].dir = FREE; 01309 motor[TIRE_BL].pwm = 30; 01310 motor[TIRE_BL].dir = BACK; 01311 case 253: 01312 motor[TIRE_FL].pwm = 25; 01313 motor[TIRE_FL].dir = FREE; 01314 motor[TIRE_BL].pwm = 40; 01315 motor[TIRE_BL].dir = BACK; 01316 break; 01317 case 254: 01318 motor[TIRE_FL].pwm = 35; 01319 motor[TIRE_FL].dir = BACK; 01320 motor[TIRE_BL].pwm = 30; 01321 motor[TIRE_BL].dir = BACK; 01322 break; 01323 case 1: 01324 motor[TIRE_FL].pwm = 35; 01325 motor[TIRE_FL].dir = FOR; 01326 motor[TIRE_BL].pwm = 25; 01327 motor[TIRE_BL].dir = FREE; 01328 break; 01329 case 3: 01330 motor[TIRE_FL].pwm = 45; 01331 motor[TIRE_FL].dir = FOR; 01332 motor[TIRE_BL].pwm = 25; 01333 motor[TIRE_BL].dir = FREE; 01334 break; 01335 case 2: 01336 motor[TIRE_FL].pwm = 35; 01337 motor[TIRE_FL].dir = FOR; 01338 motor[TIRE_BL].pwm = 30; 01339 motor[TIRE_BL].dir = FOR; 01340 break; 01341 } 01342 if(g[3]==0&&g[2]==0) { 01343 mode=25; 01344 } 01345 } else { 01346 if(g[0]==98&&countss>=40000) { 01347 mode=31; 01348 fast=60; 01349 normal=40; 01350 slow=30; 01351 countss=0; 01352 } 01353 switch(g[0]) { 01354 case 98: 01355 motor[TIRE_FR].pwm = normal; 01356 motor[TIRE_FR].dir = BACK; 01357 motor[TIRE_FL].pwm = normal+5; 01358 motor[TIRE_FL].dir = FOR; 01359 motor[TIRE_BR].pwm = normal; 01360 motor[TIRE_BR].dir = BACK; 01361 motor[TIRE_BL].pwm = normal; 01362 motor[TIRE_BL].dir = FOR; 01363 break; 01364 case 0: 01365 motor[TIRE_FR].pwm = fast; 01366 motor[TIRE_FR].dir = BACK; 01367 motor[TIRE_FL].pwm = fast+5; 01368 motor[TIRE_FL].dir = FOR; 01369 motor[TIRE_BR].pwm = fast; 01370 motor[TIRE_BR].dir = BACK; 01371 motor[TIRE_BL].pwm = fast; 01372 motor[TIRE_BL].dir = FOR; 01373 mtc=true; 01374 break; 01375 case 255: 01376 motor[TIRE_FR].pwm = fast; 01377 motor[TIRE_FR].dir = BACK; 01378 motor[TIRE_FL].pwm = slow+5; 01379 motor[TIRE_FL].dir = FOR; 01380 motor[TIRE_BR].pwm = slow; 01381 motor[TIRE_BR].dir = BACK; 01382 motor[TIRE_BL].pwm = fast; 01383 motor[TIRE_BL].dir = FOR; 01384 mtc=true; 01385 break; 01386 case 253: 01387 motor[TIRE_FR].pwm = slow; 01388 motor[TIRE_FR].dir = BACK; 01389 motor[TIRE_FL].pwm = 0; 01390 motor[TIRE_FL].dir = FREE; 01391 motor[TIRE_BR].pwm = 0; 01392 motor[TIRE_BR].dir = FREE; 01393 motor[TIRE_BL].pwm = slow; 01394 motor[TIRE_BL].dir = FOR; 01395 mtc=true; 01396 break; 01397 case 254: 01398 motor[TIRE_FR].pwm = normal; 01399 motor[TIRE_FR].dir = BACK; 01400 motor[TIRE_FL].pwm = 0; 01401 motor[TIRE_FL].dir = FREE; 01402 motor[TIRE_BR].pwm = 0; 01403 motor[TIRE_BR].dir = FREE; 01404 motor[TIRE_BL].pwm = normal; 01405 motor[TIRE_BL].dir = FOR; 01406 mtc=true; 01407 break; 01408 case 1: 01409 motor[TIRE_FR].pwm = slow; 01410 motor[TIRE_FR].dir = BACK; 01411 motor[TIRE_FL].pwm = fast+5; 01412 motor[TIRE_FL].dir = FOR; 01413 motor[TIRE_BR].pwm = fast; 01414 motor[TIRE_BR].dir = BACK; 01415 motor[TIRE_BL].pwm = slow; 01416 motor[TIRE_BL].dir = FOR; 01417 mtc=true; 01418 break; 01419 case 3: 01420 motor[TIRE_FR].pwm = 0; 01421 motor[TIRE_FR].dir = FREE; 01422 motor[TIRE_FL].pwm = slow+5; 01423 motor[TIRE_FL].dir = FOR; 01424 motor[TIRE_BR].pwm = slow; 01425 motor[TIRE_BR].dir = BACK; 01426 motor[TIRE_BL].pwm = 0; 01427 motor[TIRE_BL].dir = FREE; 01428 mtc=true; 01429 break; 01430 case 2: 01431 motor[TIRE_FR].pwm = 0; 01432 motor[TIRE_FR].dir = FREE; 01433 motor[TIRE_FL].pwm = normal+5; 01434 motor[TIRE_FL].dir = FOR; 01435 motor[TIRE_BR].pwm = normal; 01436 motor[TIRE_BR].dir = BACK; 01437 motor[TIRE_BL].pwm = 0; 01438 motor[TIRE_BL].dir = FREE; 01439 mtc=true; 01440 break; 01441 } 01442 if(g[1]!=98&&mtc==true) { 01443 switch(g[1]) { 01444 case 0: 01445 mtc=false; 01446 break; 01447 case 255: 01448 motor[TIRE_BR].pwm += 5; 01449 mtc=false; 01450 break; 01451 case 253: 01452 motor[TIRE_BR].pwm += 10; 01453 mtc=false; 01454 break; 01455 case 254: 01456 motor[TIRE_BR].pwm += 20; 01457 motor[TIRE_BL].pwm = 0; 01458 mtc=false; 01459 break; 01460 case 1: 01461 motor[TIRE_BL].pwm += 5; 01462 mtc=false; 01463 break; 01464 case 3: 01465 motor[TIRE_BL].pwm += 10; 01466 mtc=false; 01467 break; 01468 case 2: 01469 motor[TIRE_BL].pwm += 20; 01470 motor[TIRE_BR].pwm = 0; 01471 mtc=false; 01472 break; 01473 } 01474 } 01475 } 01476 break; 01477 case 25: 01478 motor[TIRE_FR].pwm = 100; 01479 motor[TIRE_FR].dir = BRAKE; 01480 motor[TIRE_FL].pwm = 100; 01481 motor[TIRE_FL].dir = BRAKE; 01482 motor[TIRE_BR].pwm = 100; 01483 motor[TIRE_BR].dir = BRAKE; 01484 motor[TIRE_BL].pwm = 100; 01485 motor[TIRE_BL].dir = BRAKE; 01486 if(DOLS==true||UPLS==true) { 01487 if(DOLS==true||UPLS==true) { 01488 motor[MOTOR_0].pwm = 200; 01489 motor[MOTOR_0].dir = BRAKE; 01490 countss=0; 01491 mode=26; 01492 } 01493 } else { 01494 motor[MOTOR_0].pwm = 100; 01495 motor[MOTOR_0].dir = BACK; 01496 } 01497 break; 01498 case 26: 01499 if(g[0]==98) { 01500 mode=31; 01501 } 01502 switch(g[4]) { 01503 case 98: 01504 motor[TIRE_FR].pwm = 60; 01505 motor[TIRE_FR].dir = BACK; 01506 motor[TIRE_FL].pwm = 65; 01507 motor[TIRE_FL].dir = FOR; 01508 motor[TIRE_BR].pwm = 60; 01509 motor[TIRE_BR].dir = BACK; 01510 motor[TIRE_BL].pwm = 60; 01511 motor[TIRE_BL].dir = FOR; 01512 break; 01513 case 99: 01514 motor[TIRE_FR].pwm = 0; 01515 motor[TIRE_FR].dir = FREE; 01516 motor[TIRE_FL].pwm = 0; 01517 motor[TIRE_FL].dir = FREE; 01518 motor[TIRE_BR].pwm = 0; 01519 motor[TIRE_BR].dir = FREE; 01520 motor[TIRE_BL].pwm = 0; 01521 motor[TIRE_BL].dir = FREE; 01522 break; 01523 case 0: 01524 motor[TIRE_FR].pwm = 60; 01525 motor[TIRE_FR].dir = BACK; 01526 motor[TIRE_FL].pwm = 65; 01527 motor[TIRE_FL].dir = FOR; 01528 motor[TIRE_BR].pwm = 60; 01529 motor[TIRE_BR].dir = BACK; 01530 motor[TIRE_BL].pwm = 60; 01531 motor[TIRE_BL].dir = FOR; 01532 break; 01533 case 255: 01534 motor[TIRE_FR].pwm = 0; 01535 motor[TIRE_FR].dir = FREE; 01536 motor[TIRE_FL].pwm = 0; 01537 motor[TIRE_FL].dir = FREE; 01538 motor[TIRE_BR].pwm = 0; 01539 motor[TIRE_BR].dir = FREE; 01540 motor[TIRE_BL].pwm = 0; 01541 motor[TIRE_BL].dir = FREE; 01542 break; 01543 case 253: 01544 motor[TIRE_FR].pwm = 60; 01545 motor[TIRE_FR].dir = BACK; 01546 motor[TIRE_FL].pwm = 30; 01547 motor[TIRE_FL].dir = FOR; 01548 motor[TIRE_BR].pwm = 25; 01549 motor[TIRE_BR].dir = BACK; 01550 motor[TIRE_BL].pwm = 60; 01551 motor[TIRE_BL].dir = FOR; 01552 break; 01553 case 254: 01554 motor[TIRE_FR].pwm = 60; 01555 motor[TIRE_FR].dir = BACK; 01556 motor[TIRE_FL].pwm = 45; 01557 motor[TIRE_FL].dir = FOR; 01558 motor[TIRE_BR].pwm = 40; 01559 motor[TIRE_BR].dir = BACK; 01560 motor[TIRE_BL].pwm = 60; 01561 motor[TIRE_BL].dir = FOR; 01562 break; 01563 case 1: 01564 motor[TIRE_FR].pwm = 0; 01565 motor[TIRE_FR].dir = FREE; 01566 motor[TIRE_FL].pwm = 0; 01567 motor[TIRE_FL].dir = FREE; 01568 motor[TIRE_BR].pwm = 0; 01569 motor[TIRE_BR].dir = FREE; 01570 motor[TIRE_BL].pwm = 0; 01571 motor[TIRE_BL].dir = FREE; 01572 break; 01573 case 3: 01574 motor[TIRE_FR].pwm = 25; 01575 motor[TIRE_FR].dir = BACK; 01576 motor[TIRE_FL].pwm = 65; 01577 motor[TIRE_FL].dir = FOR; 01578 motor[TIRE_BR].pwm = 60; 01579 motor[TIRE_BR].dir = BACK; 01580 motor[TIRE_BL].pwm = 25; 01581 motor[TIRE_BL].dir = FOR; 01582 break; 01583 case 2: 01584 motor[TIRE_FR].pwm = 40; 01585 motor[TIRE_FR].dir = BACK; 01586 motor[TIRE_FL].pwm = 65; 01587 motor[TIRE_FL].dir = FOR; 01588 motor[TIRE_BR].pwm = 60; 01589 motor[TIRE_BR].dir = BACK; 01590 motor[TIRE_BL].pwm = 40; 01591 motor[TIRE_BL].dir = FOR; 01592 break; 01593 } 01594 break; 01595 case 31: 01596 motor[TIRE_FR].pwm = 30; 01597 motor[TIRE_FR].dir = BACK; 01598 motor[TIRE_FL].pwm = 5; 01599 motor[TIRE_FL].dir = BACK; 01600 motor[TIRE_BR].pwm = 5; 01601 motor[TIRE_BR].dir = FOR; 01602 motor[TIRE_BL].pwm = 30; 01603 motor[TIRE_BL].dir = FOR; 01604 if(g[2]==0) { 01605 cross=0; 01606 countss=0; 01607 mode=40; 01608 fast=60; 01609 normal=40; 01610 slow=30; 01611 } 01612 break; 01613 case 40: 01614 switch(g[3]) { 01615 case 98: 01616 motor[TIRE_FR].pwm = normal; 01617 motor[TIRE_FR].dir = BACK; 01618 motor[TIRE_FL].pwm = normal; 01619 motor[TIRE_FL].dir = BACK; 01620 motor[TIRE_BR].pwm = normal; 01621 motor[TIRE_BR].dir = FOR; 01622 motor[TIRE_BL].pwm = normal; 01623 motor[TIRE_BL].dir = FOR; 01624 if(countss>30000) { 01625 cross++; 01626 countss=0; 01627 } 01628 mtc2=true; 01629 if(YELLOW) { 01630 if(cross==1) { 01631 mode=41; 01632 countss=0; 01633 cross=0; 01634 } 01635 } else { 01636 if(cross==3) { 01637 fast=60; 01638 normal=40; 01639 slow=30; 01640 mode=41; 01641 countss=0; 01642 cross=0; 01643 } 01644 } 01645 break; 01646 case 0: 01647 motor[TIRE_FR].pwm = fast; 01648 motor[TIRE_FR].dir = BACK; 01649 motor[TIRE_FL].pwm = fast; 01650 motor[TIRE_FL].dir = BACK; 01651 motor[TIRE_BR].pwm = fast; 01652 motor[TIRE_BR].dir = FOR; 01653 motor[TIRE_BL].pwm = fast; 01654 motor[TIRE_BL].dir = FOR; 01655 mtc2=true; 01656 break; 01657 case 255: 01658 motor[TIRE_FR].pwm = slow; 01659 motor[TIRE_FR].dir = BACK; 01660 motor[TIRE_FL].pwm = fast; 01661 motor[TIRE_FL].dir = BACK; 01662 motor[TIRE_BR].pwm = slow; 01663 motor[TIRE_BR].dir = FOR; 01664 motor[TIRE_BL].pwm = slow; 01665 motor[TIRE_BL].dir = FOR; 01666 mtc2=true; 01667 break; 01668 case 253: 01669 motor[TIRE_FR].pwm = 0; 01670 motor[TIRE_FR].dir = FREE; 01671 motor[TIRE_FL].pwm = slow; 01672 motor[TIRE_FL].dir = BACK; 01673 motor[TIRE_BR].pwm = slow; 01674 motor[TIRE_BR].dir = FOR; 01675 motor[TIRE_BL].pwm = 0; 01676 motor[TIRE_BL].dir = FREE; 01677 mtc2=true; 01678 break; 01679 case 254: 01680 motor[TIRE_FR].pwm = 0; 01681 motor[TIRE_FR].dir = FREE; 01682 motor[TIRE_FL].pwm = normal; 01683 motor[TIRE_FL].dir = BACK; 01684 motor[TIRE_BR].pwm = normal; 01685 motor[TIRE_BR].dir = FOR; 01686 motor[TIRE_BL].pwm = 0; 01687 motor[TIRE_BL].dir = FREE; 01688 mtc2=true; 01689 break; 01690 case 1: 01691 motor[TIRE_FR].pwm = slow; 01692 motor[TIRE_FR].dir = BACK; 01693 motor[TIRE_FL].pwm = slow; 01694 motor[TIRE_FL].dir = BACK; 01695 motor[TIRE_BR].pwm = slow; 01696 motor[TIRE_BR].dir = FOR; 01697 motor[TIRE_BL].pwm = fast; 01698 motor[TIRE_BL].dir = FOR; 01699 mtc2=true; 01700 break; 01701 case 3: 01702 motor[TIRE_FR].pwm = slow; 01703 motor[TIRE_FR].dir = BACK; 01704 motor[TIRE_FL].pwm = 0; 01705 motor[TIRE_FL].dir = FREE; 01706 motor[TIRE_BR].pwm = 0; 01707 motor[TIRE_BR].dir = FREE; 01708 motor[TIRE_BL].pwm = slow; 01709 motor[TIRE_BL].dir = FOR; 01710 mtc2=true; 01711 break; 01712 case 2: 01713 motor[TIRE_FR].pwm = normal; 01714 motor[TIRE_FR].dir = BACK; 01715 motor[TIRE_FL].pwm = 0; 01716 motor[TIRE_FL].dir = FREE; 01717 motor[TIRE_BR].pwm = 0; 01718 motor[TIRE_BR].dir = FREE; 01719 motor[TIRE_BL].pwm = normal; 01720 motor[TIRE_BL].dir = FOR; 01721 mtc2=true; 01722 break; 01723 } 01724 if(g[2]!=98&&mtc2==true) { 01725 switch(g[2]) { 01726 case 0: 01727 mtc2=false; 01728 break; 01729 case 255: 01730 motor[TIRE_FL].pwm += 5; 01731 mtc2=false; 01732 break; 01733 case 253: 01734 motor[TIRE_FL].pwm += 10; 01735 mtc2=false; 01736 break; 01737 case 254: 01738 motor[TIRE_FL].pwm += 20; 01739 motor[TIRE_BL].pwm = 0; 01740 mtc2=false; 01741 break; 01742 case 1: 01743 motor[TIRE_BL].pwm += 5; 01744 mtc2=false; 01745 break; 01746 case 3: 01747 motor[TIRE_BL].pwm += 10; 01748 mtc2=false; 01749 break; 01750 case 2: 01751 motor[TIRE_BL].pwm += 20; 01752 motor[TIRE_FL].pwm = 0; 01753 mtc2=false; 01754 break; 01755 } 01756 } 01757 break; 01758 case 41: 01759 /* 01760 if(LIF||LIB) { 01761 mode=42; 01762 } 01763 */ 01764 motor[TIRE_FR].pwm = 30; 01765 motor[TIRE_FR].dir = BACK; 01766 motor[TIRE_FL].pwm = 0; 01767 motor[TIRE_FL].dir = FREE; 01768 motor[TIRE_BR].pwm = 0; 01769 motor[TIRE_BR].dir = FREE; 01770 motor[TIRE_BL].pwm = 30; 01771 motor[TIRE_BL].dir = FOR; 01772 if(g[1]==0) { 01773 countss=0; 01774 mode=70; 01775 normal=60; 01776 fast=70; 01777 slow=40; 01778 } 01779 break; 01780 case 42: 01781 motor[TIRE_FR].pwm = 40; 01782 motor[TIRE_FR].dir = FOR; 01783 motor[TIRE_FL].pwm = 0; 01784 motor[TIRE_FL].dir = FREE; 01785 motor[TIRE_BR].pwm = 0; 01786 motor[TIRE_BR].dir = FREE; 01787 motor[TIRE_BL].pwm = 40; 01788 motor[TIRE_BL].dir = BACK; 01789 if(g[1]==0) { 01790 countss=0; 01791 mode=70; 01792 } 01793 break; 01794 case 70: 01795 switch(g[0]) { 01796 case 99: 01797 if(countss>=100000) { 01798 mode=100; 01799 } 01800 break; 01801 case 98: 01802 motor[TIRE_FR].pwm = normal; 01803 motor[TIRE_FR].dir = BACK; 01804 motor[TIRE_FL].pwm = normal; 01805 motor[TIRE_FL].dir = FOR; 01806 motor[TIRE_BR].pwm = normal; 01807 motor[TIRE_BR].dir = BACK; 01808 motor[TIRE_BL].pwm = normal; 01809 motor[TIRE_BL].dir = FOR; 01810 break; 01811 case 0: 01812 motor[TIRE_FR].pwm = 80; 01813 motor[TIRE_FR].dir = BACK; 01814 motor[TIRE_FL].pwm = 80; 01815 motor[TIRE_FL].dir = FOR; 01816 motor[TIRE_BR].pwm = 80; 01817 motor[TIRE_BR].dir = BACK; 01818 motor[TIRE_BL].pwm = 80; 01819 motor[TIRE_BL].dir = FOR; 01820 mtc=true; 01821 break; 01822 case 255: 01823 motor[TIRE_FR].pwm = fast; 01824 motor[TIRE_FR].dir = BACK; 01825 motor[TIRE_FL].pwm = slow; 01826 motor[TIRE_FL].dir = FOR; 01827 motor[TIRE_BR].pwm = slow; 01828 motor[TIRE_BR].dir = BACK; 01829 motor[TIRE_BL].pwm = fast; 01830 motor[TIRE_BL].dir = FOR; 01831 mtc=true; 01832 break; 01833 case 253: 01834 motor[TIRE_FR].pwm = slow; 01835 motor[TIRE_FR].dir = BACK; 01836 motor[TIRE_FL].pwm = 0; 01837 motor[TIRE_FL].dir = FREE; 01838 motor[TIRE_BR].pwm = 0; 01839 motor[TIRE_BR].dir = FREE; 01840 motor[TIRE_BL].pwm = slow; 01841 motor[TIRE_BL].dir = FOR; 01842 mtc=true; 01843 break; 01844 case 254: 01845 motor[TIRE_FR].pwm = normal; 01846 motor[TIRE_FR].dir = BACK; 01847 motor[TIRE_FL].pwm = 0; 01848 motor[TIRE_FL].dir = FREE; 01849 motor[TIRE_BR].pwm = 0; 01850 motor[TIRE_BR].dir = FREE; 01851 motor[TIRE_BL].pwm = normal; 01852 motor[TIRE_BL].dir = FOR; 01853 mtc=true; 01854 break; 01855 case 1: 01856 motor[TIRE_FR].pwm = slow; 01857 motor[TIRE_FR].dir = BACK; 01858 motor[TIRE_FL].pwm = fast; 01859 motor[TIRE_FL].dir = FOR; 01860 motor[TIRE_BR].pwm = fast; 01861 motor[TIRE_BR].dir = BACK; 01862 motor[TIRE_BL].pwm = slow; 01863 motor[TIRE_BL].dir = FOR; 01864 mtc=true; 01865 break; 01866 case 3: 01867 motor[TIRE_FR].pwm = 0; 01868 motor[TIRE_FR].dir = FREE; 01869 motor[TIRE_FL].pwm = slow; 01870 motor[TIRE_FL].dir = FOR; 01871 motor[TIRE_BR].pwm = slow; 01872 motor[TIRE_BR].dir = BACK; 01873 motor[TIRE_BL].pwm = 0; 01874 motor[TIRE_BL].dir = FREE; 01875 mtc=true; 01876 break; 01877 case 2: 01878 motor[TIRE_FR].pwm = 0; 01879 motor[TIRE_FR].dir = FREE; 01880 motor[TIRE_FL].pwm = normal; 01881 motor[TIRE_FL].dir = FOR; 01882 motor[TIRE_BR].pwm = normal; 01883 motor[TIRE_BR].dir = BACK; 01884 motor[TIRE_BL].pwm = 0; 01885 motor[TIRE_BL].dir = FREE; 01886 mtc=true; 01887 break; 01888 } 01889 if(g[1]!=98&&mtc==true) { 01890 switch(g[1]) { 01891 case 0: 01892 mtc=false; 01893 break; 01894 case 255: 01895 motor[TIRE_BR].pwm += 5; 01896 mtc=false; 01897 break; 01898 case 253: 01899 motor[TIRE_BR].pwm += 10; 01900 mtc=false; 01901 break; 01902 case 254: 01903 motor[TIRE_BR].pwm += 20; 01904 motor[TIRE_BL].pwm = 0; 01905 mtc=false; 01906 break; 01907 case 1: 01908 motor[TIRE_BL].pwm += 5; 01909 mtc=false; 01910 break; 01911 case 3: 01912 motor[TIRE_BL].pwm += 10; 01913 mtc=false; 01914 break; 01915 case 2: 01916 motor[TIRE_BL].pwm += 20; 01917 motor[TIRE_BR].pwm = 0; 01918 mtc=false; 01919 break; 01920 } 01921 } 01922 break; 01923 case 100: 01924 motor[TIRE_FR].pwm = 50; 01925 motor[TIRE_FR].dir = BACK; 01926 motor[TIRE_FL].pwm = 50; 01927 motor[TIRE_FL].dir = FOR; 01928 motor[TIRE_BR].pwm = 50; 01929 motor[TIRE_BR].dir = BACK; 01930 motor[TIRE_BL].pwm = 50; 01931 motor[TIRE_BL].dir = FOR; 01932 break; 01933 default: 01934 motor[TIRE_FR].pwm = 50; 01935 motor[TIRE_FR].dir = BRAKE; 01936 motor[TIRE_FL].pwm = 50; 01937 motor[TIRE_FL].dir = BRAKE; 01938 motor[TIRE_BR].pwm = 50; 01939 motor[TIRE_BR].dir = BRAKE; 01940 motor[TIRE_BL].pwm = 50; 01941 motor[TIRE_BL].dir = BRAKE; 01942 break; 01943 } 01944 } 01945 #endif 01946 01947 #if USE_PROCESS_NUM>3 01948 static void Process3() 01949 { 01950 //Clearly 赤ゾーン 01951 LED::LED::TapeledMode(3); 01952 countss++; 01953 if(mode==1) {//スタートゾーンから白線検知 01954 fast=40; 01955 normal=30; 01956 slow=20; 01957 motor[TIRE_FR].pwm = normal+20; 01958 motor[TIRE_FR].dir = BACK; 01959 motor[TIRE_FL].pwm = normal+20; 01960 motor[TIRE_FL].dir = FOR; 01961 motor[TIRE_BR].pwm = normal+20; 01962 motor[TIRE_BR].dir = BACK; 01963 motor[TIRE_BL].pwm = normal+20; 01964 motor[TIRE_BL].dir = FOR; 01965 if(g[2]==0&&countss>=100000) { 01966 countss=100000; 01967 cross=0; 01968 mode=3; 01969 } 01970 } else if(mode==3) { //横ライントレースから縦ライントレースへ 01971 motor[TIRE_FR].pwm = 255; 01972 motor[TIRE_FR].dir = BRAKE; 01973 motor[TIRE_FL].pwm = 30; 01974 motor[TIRE_FL].dir = FOR; 01975 motor[TIRE_BR].pwm = 30; 01976 motor[TIRE_BR].dir = BACK; 01977 motor[TIRE_BL].pwm = 255; 01978 motor[TIRE_BL].dir = BRAKE; 01979 if(g[0]==0) { 01980 mode=10; 01981 countss=100000; 01982 } 01983 } else if(mode==10) { //縦ライントレース 01984 switch(g[0]) { 01985 case 98: 01986 motor[TIRE_FR].pwm = normal+50; 01987 motor[TIRE_FR].dir = BACK; 01988 motor[TIRE_FL].pwm = normal+50; 01989 motor[TIRE_FL].dir = FOR; 01990 motor[TIRE_BR].pwm = normal+50; 01991 motor[TIRE_BR].dir = BACK; 01992 motor[TIRE_BL].pwm = normal+50; 01993 motor[TIRE_BL].dir = FOR; 01994 mtc=true; 01995 if(countss>80000) { 01996 cross++; 01997 countss=0; 01998 } 01999 if(cross==2) { 02000 fast = 10; 02001 normal = 10; 02002 slow = 5; 02003 } 02004 02005 if(cross==3) { 02006 fast=40; 02007 normal=30; 02008 slow=20; 02009 mode=11; 02010 countss=0; 02011 cross=0; 02012 02013 } 02014 break; 02015 case 0: 02016 motor[TIRE_FR].pwm = fast+50; 02017 motor[TIRE_FR].dir = BACK; 02018 motor[TIRE_FL].pwm = fast+50; 02019 motor[TIRE_FL].dir = FOR; 02020 motor[TIRE_BR].pwm = fast+50; 02021 motor[TIRE_BR].dir = BACK; 02022 motor[TIRE_BL].pwm = fast+50; 02023 motor[TIRE_BL].dir = FOR; 02024 mtc=true; 02025 break; 02026 case 255: 02027 motor[TIRE_FR].pwm = fast+50; 02028 motor[TIRE_FR].dir = BACK; 02029 motor[TIRE_FL].pwm = slow+50; 02030 motor[TIRE_FL].dir = FOR; 02031 motor[TIRE_BR].pwm = slow+50; 02032 motor[TIRE_BR].dir = BACK; 02033 motor[TIRE_BL].pwm = fast+50; 02034 motor[TIRE_BL].dir = FOR; 02035 mtc=true; 02036 break; 02037 case 253: 02038 motor[TIRE_FR].pwm = slow+50; 02039 motor[TIRE_FR].dir = BACK; 02040 motor[TIRE_FL].pwm = 0; 02041 motor[TIRE_FL].dir = FREE; 02042 motor[TIRE_BR].pwm = 0; 02043 motor[TIRE_BR].dir = FREE; 02044 motor[TIRE_BL].pwm = slow+50; 02045 motor[TIRE_BL].dir = FOR; 02046 mtc=true; 02047 break; 02048 case 254: 02049 motor[TIRE_FR].pwm = normal+50; 02050 motor[TIRE_FR].dir = BACK; 02051 motor[TIRE_FL].pwm = 0; 02052 motor[TIRE_FL].dir = FREE; 02053 motor[TIRE_BR].pwm = 0; 02054 motor[TIRE_BR].dir = FREE; 02055 motor[TIRE_BL].pwm = normal+50; 02056 motor[TIRE_BL].dir = FOR; 02057 mtc=true; 02058 break; 02059 case 1: 02060 motor[TIRE_FR].pwm = slow+50; 02061 motor[TIRE_FR].dir = BACK; 02062 motor[TIRE_FL].pwm = fast+50; 02063 motor[TIRE_FL].dir = FOR; 02064 motor[TIRE_BR].pwm = fast+50; 02065 motor[TIRE_BR].dir = BACK; 02066 motor[TIRE_BL].pwm = slow+50; 02067 motor[TIRE_BL].dir = FOR; 02068 mtc=true; 02069 break; 02070 case 3: 02071 motor[TIRE_FR].pwm = 0; 02072 motor[TIRE_FR].dir = FREE; 02073 motor[TIRE_FL].pwm = slow+50; 02074 motor[TIRE_FL].dir = FOR; 02075 motor[TIRE_BR].pwm = slow+50; 02076 motor[TIRE_BR].dir = BACK; 02077 motor[TIRE_BL].pwm = 0; 02078 motor[TIRE_BL].dir = FREE; 02079 mtc=true; 02080 break; 02081 case 2: 02082 motor[TIRE_FR].pwm = 0; 02083 motor[TIRE_FR].dir = FREE; 02084 motor[TIRE_FL].pwm = normal+50; 02085 motor[TIRE_FL].dir = FOR; 02086 motor[TIRE_BR].pwm = normal+50; 02087 motor[TIRE_BR].dir = BACK; 02088 motor[TIRE_BL].pwm = 0; 02089 motor[TIRE_BL].dir = FREE; 02090 mtc=true; 02091 break; 02092 } 02093 if(g[0]!=98&&mtc==true) { 02094 switch(g[1]) { 02095 case 0: 02096 mtc=false; 02097 break; 02098 case 255: 02099 motor[TIRE_BR].pwm += 3; 02100 mtc=false; 02101 break; 02102 case 253: 02103 motor[TIRE_BR].pwm += 6; 02104 mtc=false; 02105 break; 02106 case 254: 02107 motor[TIRE_BR].pwm += 9; 02108 motor[TIRE_BL].pwm = 0; 02109 mtc=false; 02110 break; 02111 case 1: 02112 motor[TIRE_BL].pwm += 3; 02113 mtc=false; 02114 break; 02115 case 3: 02116 motor[TIRE_BL].pwm += 6; 02117 mtc=false; 02118 break; 02119 case 2: 02120 motor[TIRE_BL].pwm += 9; 02121 motor[TIRE_BR].pwm = 0; 02122 mtc=false; 02123 break; 02124 } 02125 } 02126 } else if(mode==11) { 02127 motor[TIRE_FR].pwm = 25; 02128 motor[TIRE_FR].dir = FOR; 02129 motor[TIRE_FL].pwm = normal; 02130 motor[TIRE_FL].dir = FOR; 02131 motor[TIRE_BR].pwm = normal; 02132 motor[TIRE_BR].dir = BACK; 02133 motor[TIRE_BL].pwm = 25; 02134 motor[TIRE_BL].dir = BACK; 02135 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { 02136 countss=0; 02137 cross=0; 02138 mode=20; 02139 } 02140 } else if(mode==20) { 02141 solenoid.solenoid4= SOLENOID_ON; 02142 switch(g[2]) { 02143 case 98: 02144 motor[TIRE_FR].pwm = normal+20; 02145 motor[TIRE_FR].dir = FOR; 02146 motor[TIRE_FL].pwm = normal+20; 02147 motor[TIRE_FL].dir = FOR; 02148 motor[TIRE_BR].pwm = normal+20; 02149 motor[TIRE_BR].dir = BACK; 02150 motor[TIRE_BL].pwm = normal+20; 02151 motor[TIRE_BL].dir = BACK; 02152 if(countss>30000) { 02153 cross++; 02154 countss=0; 02155 } 02156 mtc2=true; 02157 02158 if(YELLOW) { 02159 if(cross==1) { 02160 fast=20; 02161 normal=15; 02162 slow=15; 02163 } else if(cross==2) { 02164 mode=21; 02165 countss=0; 02166 cross=0; 02167 } 02168 } else { 02169 if(cross==3) { 02170 mode=21; 02171 countss=0; 02172 cross=0; 02173 } 02174 } 02175 break; 02176 case 0: 02177 motor[TIRE_FR].pwm = fast+20; 02178 motor[TIRE_FR].dir = FOR; 02179 motor[TIRE_FL].pwm = fast+20; 02180 motor[TIRE_FL].dir = FOR; 02181 motor[TIRE_BR].pwm = fast+20; 02182 motor[TIRE_BR].dir = BACK; 02183 motor[TIRE_BL].pwm = fast+20; 02184 motor[TIRE_BL].dir = BACK; 02185 mtc2=true; 02186 break; 02187 case 255: 02188 motor[TIRE_FR].pwm = normal+20; 02189 motor[TIRE_FR].dir = FOR; 02190 motor[TIRE_FL].pwm = fast+20; 02191 motor[TIRE_FL].dir = FOR; 02192 motor[TIRE_BR].pwm = fast+20; 02193 motor[TIRE_BR].dir = BACK; 02194 motor[TIRE_BL].pwm = normal+20; 02195 motor[TIRE_BL].dir = BACK; 02196 mtc2=true; 02197 break; 02198 case 253: 02199 motor[TIRE_FR].pwm = 0; 02200 motor[TIRE_FR].dir = FREE; 02201 motor[TIRE_FL].pwm = slow+20; 02202 motor[TIRE_FL].dir = FOR; 02203 motor[TIRE_BR].pwm = slow+20; 02204 motor[TIRE_BR].dir = BACK; 02205 motor[TIRE_BL].pwm = 0; 02206 motor[TIRE_BL].dir = FREE; 02207 mtc2=true; 02208 break; 02209 case 254: 02210 motor[TIRE_FR].pwm = 0; 02211 motor[TIRE_FR].dir = FREE; 02212 motor[TIRE_FL].pwm = normal+20; 02213 motor[TIRE_FL].dir = FOR; 02214 motor[TIRE_BR].pwm = normal+20; 02215 motor[TIRE_BR].dir = BACK; 02216 motor[TIRE_BL].pwm = 0; 02217 motor[TIRE_BL].dir = FREE; 02218 mtc2=true; 02219 break; 02220 case 1: 02221 motor[TIRE_FR].pwm = fast+20; 02222 motor[TIRE_FR].dir = FOR; 02223 motor[TIRE_FL].pwm = slow+20; 02224 motor[TIRE_FL].dir = FOR; 02225 motor[TIRE_BR].pwm = slow+20; 02226 motor[TIRE_BR].dir = BACK; 02227 motor[TIRE_BL].pwm = fast+20; 02228 motor[TIRE_BL].dir = BACK; 02229 mtc2=true; 02230 break; 02231 case 3: 02232 motor[TIRE_FR].pwm = slow+20; 02233 motor[TIRE_FR].dir = FOR; 02234 motor[TIRE_FL].pwm = 0; 02235 motor[TIRE_FL].dir = FREE; 02236 motor[TIRE_BR].pwm = 0; 02237 motor[TIRE_BR].dir = FREE; 02238 motor[TIRE_BL].pwm = slow+20; 02239 motor[TIRE_BL].dir = BACK; 02240 mtc2=true; 02241 break; 02242 case 2: 02243 motor[TIRE_FR].pwm = normal+20; 02244 motor[TIRE_FR].dir = FOR; 02245 motor[TIRE_FL].pwm = 0; 02246 motor[TIRE_FL].dir = FREE; 02247 motor[TIRE_BR].pwm = 0; 02248 motor[TIRE_BR].dir = FREE; 02249 motor[TIRE_BL].pwm = normal+20; 02250 motor[TIRE_BL].dir = BACK; 02251 mtc2=true; 02252 break; 02253 } 02254 if(g[2]!=98&&mtc2==true) { 02255 switch(g[3]) { 02256 case 0: 02257 mtc2=false; 02258 break; 02259 case 255: 02260 motor[TIRE_BL].pwm += 5; 02261 mtc2=false; 02262 break; 02263 case 253: 02264 motor[TIRE_BL].pwm += 10; 02265 mtc2=false; 02266 break; 02267 case 254: 02268 motor[TIRE_BL].pwm += 15; 02269 motor[TIRE_FL].pwm = 0; 02270 mtc2=false; 02271 break; 02272 case 1: 02273 motor[TIRE_FL].pwm += 5; 02274 mtc2=false; 02275 break; 02276 case 3: 02277 motor[TIRE_FL].pwm += 10; 02278 mtc2=false; 02279 break; 02280 case 2: 02281 motor[TIRE_FL].pwm += 15; 02282 motor[TIRE_BL].pwm = 0; 02283 mtc2=false; 02284 break; 02285 } 02286 } 02287 } 02288 if(YELLOW) { 02289 if(mode==21) { 02290 motor[TIRE_FR].pwm = 20; 02291 motor[TIRE_FR].dir = BACK; 02292 motor[TIRE_FL].pwm = 20; 02293 motor[TIRE_FL].dir = BACK; 02294 motor[TIRE_BR].pwm = 20; 02295 motor[TIRE_BR].dir = FOR; 02296 motor[TIRE_BL].pwm = 20; 02297 motor[TIRE_BL].dir = FOR; 02298 if(g[3]==98&&countss>=10000) { 02299 mode=22; 02300 countss=0; 02301 fast=60; 02302 normal=40; 02303 slow=20; 02304 } 02305 } else if(mode==22) { 02306 if(g[4]==3||g[4]==2) { 02307 motor[TIRE_FR].pwm = 30; 02308 motor[TIRE_FR].dir = FOR; 02309 motor[TIRE_FL].pwm = 30; 02310 motor[TIRE_FL].dir = FOR; 02311 motor[TIRE_BR].pwm = 30; 02312 motor[TIRE_BR].dir = BACK; 02313 motor[TIRE_BL].pwm = 30; 02314 motor[TIRE_BL].dir = BACK; 02315 } 02316 if(g[4]==253||g[4]==254) { 02317 motor[TIRE_FR].pwm = 30; 02318 motor[TIRE_FR].dir = BACK; 02319 motor[TIRE_FL].pwm = 30; 02320 motor[TIRE_FL].dir = BACK; 02321 motor[TIRE_BR].pwm = 30; 02322 motor[TIRE_BR].dir = FOR; 02323 motor[TIRE_BL].pwm = 30; 02324 motor[TIRE_BL].dir = FOR; 02325 } 02326 switch(g[2]) { 02327 case 0: 02328 motor[TIRE_FR].pwm = 20; 02329 motor[TIRE_FR].dir = FREE; 02330 motor[TIRE_BR].pwm = 20; 02331 motor[TIRE_BR].dir = FREE; 02332 break; 02333 case 255: 02334 motor[TIRE_FR].pwm = 30; 02335 motor[TIRE_FR].dir = BACK; 02336 motor[TIRE_BR].pwm = 20; 02337 motor[TIRE_BR].dir = FREE; 02338 break; 02339 case 253: 02340 motor[TIRE_FR].pwm = 40; 02341 motor[TIRE_FR].dir = BACK; 02342 motor[TIRE_BR].pwm = 20; 02343 motor[TIRE_BR].dir = FREE; 02344 break; 02345 case 254: 02346 motor[TIRE_FR].pwm = 30; 02347 motor[TIRE_FR].dir = BACK; 02348 motor[TIRE_BR].pwm = 30; 02349 motor[TIRE_BR].dir = BACK; 02350 break; 02351 case 1: 02352 motor[TIRE_FR].pwm = 20; 02353 motor[TIRE_FR].dir = FREE; 02354 motor[TIRE_BR].pwm = 30; 02355 motor[TIRE_BR].dir = FOR; 02356 break; 02357 case 3: 02358 motor[TIRE_FR].pwm = 20; 02359 motor[TIRE_FR].dir = FREE; 02360 motor[TIRE_BR].pwm = 40; 02361 motor[TIRE_BR].dir = FOR; 02362 break; 02363 case 2: 02364 motor[TIRE_FR].pwm = 30; 02365 motor[TIRE_FR].dir = FOR; 02366 motor[TIRE_BR].pwm = 30; 02367 motor[TIRE_BR].dir = FOR; 02368 break; 02369 } 02370 switch(g[3]) { 02371 case 0: 02372 motor[TIRE_FL].pwm = 20; 02373 motor[TIRE_FL].dir = FREE; 02374 motor[TIRE_BL].pwm = 20; 02375 motor[TIRE_BL].dir = FREE; 02376 break; 02377 case 255: 02378 motor[TIRE_FL].pwm = 20; 02379 motor[TIRE_FL].dir = FREE; 02380 motor[TIRE_BL].pwm = 30; 02381 motor[TIRE_BL].dir = BACK; 02382 case 253: 02383 motor[TIRE_FL].pwm = 20; 02384 motor[TIRE_FL].dir = FREE; 02385 motor[TIRE_BL].pwm = 40; 02386 motor[TIRE_BL].dir = BACK; 02387 break; 02388 case 254: 02389 motor[TIRE_FL].pwm = 30; 02390 motor[TIRE_FL].dir = BACK; 02391 motor[TIRE_BL].pwm = 30; 02392 motor[TIRE_BL].dir = BACK; 02393 break; 02394 case 1: 02395 motor[TIRE_FL].pwm = 30; 02396 motor[TIRE_FL].dir = FOR; 02397 motor[TIRE_BL].pwm = 20; 02398 motor[TIRE_BL].dir = FREE; 02399 break; 02400 case 3: 02401 motor[TIRE_FL].pwm = 40; 02402 motor[TIRE_FL].dir = FOR; 02403 motor[TIRE_BL].pwm = 20; 02404 motor[TIRE_BL].dir = FREE; 02405 break; 02406 case 2: 02407 motor[TIRE_FL].pwm = 30; 02408 motor[TIRE_FL].dir = FOR; 02409 motor[TIRE_BL].pwm = 30; 02410 motor[TIRE_BL].dir = FOR; 02411 break; 02412 } 02413 if(g[3]==0&&g[2]==0) { 02414 mode=23; 02415 countss=0; 02416 } 02417 } else if(mode==23) { 02418 switch(g[4]) { 02419 case 98: 02420 motor[TIRE_FR].pwm = fast-5; 02421 motor[TIRE_FR].dir = FOR; 02422 motor[TIRE_FL].pwm = fast-5; 02423 motor[TIRE_FL].dir = BACK; 02424 motor[TIRE_BR].pwm = fast-5; 02425 motor[TIRE_BR].dir = FOR; 02426 motor[TIRE_BL].pwm = fast-5; 02427 motor[TIRE_BL].dir = BACK; 02428 break; 02429 case 99: 02430 motor[TIRE_FR].pwm = 0; 02431 motor[TIRE_FR].dir = FREE; 02432 motor[TIRE_FL].pwm = 0; 02433 motor[TIRE_FL].dir = FREE; 02434 motor[TIRE_BR].pwm = 0; 02435 motor[TIRE_BR].dir = FREE; 02436 motor[TIRE_BL].pwm = 0; 02437 motor[TIRE_BL].dir = FREE; 02438 break; 02439 case 0: 02440 motor[TIRE_FR].pwm = fast-5; 02441 motor[TIRE_FR].dir = FOR; 02442 motor[TIRE_FL].pwm = fast-5; 02443 motor[TIRE_FL].dir = BACK; 02444 motor[TIRE_BR].pwm = fast-5; 02445 motor[TIRE_BR].dir = FOR; 02446 motor[TIRE_BL].pwm = fast-5; 02447 motor[TIRE_BL].dir = BACK; 02448 break; 02449 case 255: 02450 motor[TIRE_FR].pwm = 0; 02451 motor[TIRE_FR].dir = FREE; 02452 motor[TIRE_FL].pwm = 0; 02453 motor[TIRE_FL].dir = FREE; 02454 motor[TIRE_BR].pwm = 0; 02455 motor[TIRE_BR].dir = FREE; 02456 motor[TIRE_BL].pwm = 0; 02457 motor[TIRE_BL].dir = FREE; 02458 break; 02459 case 253: 02460 motor[TIRE_FR].pwm = slow+5; 02461 motor[TIRE_FR].dir = FOR; 02462 motor[TIRE_FL].pwm = fast-5; 02463 motor[TIRE_FL].dir = BACK; 02464 motor[TIRE_BR].pwm = fast-5; 02465 motor[TIRE_BR].dir = FOR; 02466 motor[TIRE_BL].pwm = slow+5; 02467 motor[TIRE_BL].dir = BACK; 02468 break; 02469 case 254: 02470 motor[TIRE_FR].pwm = normal+5; 02471 motor[TIRE_FR].dir = FOR; 02472 motor[TIRE_FL].pwm = fast-5; 02473 motor[TIRE_FL].dir = BACK; 02474 motor[TIRE_BR].pwm = fast-5; 02475 motor[TIRE_BR].dir = FOR; 02476 motor[TIRE_BL].pwm = normal+5; 02477 motor[TIRE_BL].dir = BACK; 02478 break; 02479 case 1: 02480 motor[TIRE_FR].pwm = 0; 02481 motor[TIRE_FR].dir = FREE; 02482 motor[TIRE_FL].pwm = 0; 02483 motor[TIRE_FL].dir = FREE; 02484 motor[TIRE_BR].pwm = 0; 02485 motor[TIRE_BR].dir = FREE; 02486 motor[TIRE_BL].pwm = 0; 02487 motor[TIRE_BL].dir = FREE; 02488 break; 02489 case 3: 02490 motor[TIRE_FR].pwm = fast-5; 02491 motor[TIRE_FR].dir = FOR; 02492 motor[TIRE_FL].pwm = slow+5; 02493 motor[TIRE_FL].dir = BACK; 02494 motor[TIRE_BR].pwm = slow+5; 02495 motor[TIRE_BR].dir = FOR; 02496 motor[TIRE_BL].pwm = fast-5; 02497 motor[TIRE_BL].dir = BACK; 02498 break; 02499 case 2: 02500 motor[TIRE_FR].pwm = fast-5; 02501 motor[TIRE_FR].dir = FOR; 02502 motor[TIRE_FL].pwm = normal+5; 02503 motor[TIRE_FL].dir = BACK; 02504 motor[TIRE_BR].pwm = normal+5; 02505 motor[TIRE_BR].dir = FOR; 02506 motor[TIRE_BL].pwm = fast-5; 02507 motor[TIRE_BL].dir = BACK; 02508 break; 02509 } 02510 if(g[1]==98) { 02511 fast=20; 02512 normal=15; 02513 slow=10; 02514 } 02515 if(g[3]==254&&countss>=100000) { 02516 mode=24; 02517 solenoid.solenoid4 = SOLENOID_OFF; 02518 countss=0; 02519 fast=60; 02520 normal=40; 02521 slow=20; 02522 } 02523 } else if(mode==24) { 02524 if(g[4]==3||g[4]==2) { 02525 motor[TIRE_FR].pwm = 30; 02526 motor[TIRE_FR].dir = FOR; 02527 motor[TIRE_FL].pwm = 30; 02528 motor[TIRE_FL].dir = FOR; 02529 motor[TIRE_BR].pwm = 30; 02530 motor[TIRE_BR].dir = BACK; 02531 motor[TIRE_BL].pwm = 30; 02532 motor[TIRE_BL].dir = BACK; 02533 } 02534 if(g[4]==253||g[4]==254) { 02535 motor[TIRE_FR].pwm = 30; 02536 motor[TIRE_FR].dir = BACK; 02537 motor[TIRE_FL].pwm = 30; 02538 motor[TIRE_FL].dir = BACK; 02539 motor[TIRE_BR].pwm = 30; 02540 motor[TIRE_BR].dir = FOR; 02541 motor[TIRE_BL].pwm = 30; 02542 motor[TIRE_BL].dir = FOR; 02543 } 02544 switch(g[2]) { 02545 case 0: 02546 motor[TIRE_FR].pwm = 25; 02547 motor[TIRE_FR].dir = FREE; 02548 motor[TIRE_BR].pwm = 25; 02549 motor[TIRE_BR].dir = FREE; 02550 break; 02551 case 255: 02552 motor[TIRE_FR].pwm = 30; 02553 motor[TIRE_FR].dir = BACK; 02554 motor[TIRE_BR].pwm = 25; 02555 motor[TIRE_BR].dir = FREE; 02556 break; 02557 case 253: 02558 motor[TIRE_FR].pwm = 40; 02559 motor[TIRE_FR].dir = BACK; 02560 motor[TIRE_BR].pwm = 25; 02561 motor[TIRE_BR].dir = FREE; 02562 break; 02563 case 254: 02564 motor[TIRE_FR].pwm = 30; 02565 motor[TIRE_FR].dir = BACK; 02566 motor[TIRE_BR].pwm = 30; 02567 motor[TIRE_BR].dir = BACK; 02568 break; 02569 case 1: 02570 motor[TIRE_FR].pwm = 25; 02571 motor[TIRE_FR].dir = FREE; 02572 motor[TIRE_BR].pwm = 30; 02573 motor[TIRE_BR].dir = FOR; 02574 break; 02575 case 3: 02576 motor[TIRE_FR].pwm = 25; 02577 motor[TIRE_FR].dir = FREE; 02578 motor[TIRE_BR].pwm = 40; 02579 motor[TIRE_BR].dir = FOR; 02580 break; 02581 case 2: 02582 motor[TIRE_FR].pwm = 30; 02583 motor[TIRE_FR].dir = FOR; 02584 motor[TIRE_BR].pwm = 30; 02585 motor[TIRE_BR].dir = FOR; 02586 break; 02587 } 02588 switch(g[3]) { 02589 case 0: 02590 motor[TIRE_FL].pwm = 25; 02591 motor[TIRE_FL].dir = FREE; 02592 motor[TIRE_BL].pwm = 25; 02593 motor[TIRE_BL].dir = FREE; 02594 break; 02595 case 255: 02596 motor[TIRE_FL].pwm = 25; 02597 motor[TIRE_FL].dir = FREE; 02598 motor[TIRE_BL].pwm = 30; 02599 motor[TIRE_BL].dir = BACK; 02600 case 253: 02601 motor[TIRE_FL].pwm = 25; 02602 motor[TIRE_FL].dir = FREE; 02603 motor[TIRE_BL].pwm = 40; 02604 motor[TIRE_BL].dir = BACK; 02605 break; 02606 case 254: 02607 motor[TIRE_FL].pwm = 30; 02608 motor[TIRE_FL].dir = BACK; 02609 motor[TIRE_BL].pwm = 30; 02610 motor[TIRE_BL].dir = BACK; 02611 break; 02612 case 1: 02613 motor[TIRE_FL].pwm = 30; 02614 motor[TIRE_FL].dir = FOR; 02615 motor[TIRE_BL].pwm = 25; 02616 motor[TIRE_BL].dir = FREE; 02617 break; 02618 case 3: 02619 motor[TIRE_FL].pwm = 40; 02620 motor[TIRE_FL].dir = FOR; 02621 motor[TIRE_BL].pwm = 25; 02622 motor[TIRE_BL].dir = FREE; 02623 break; 02624 case 2: 02625 motor[TIRE_FL].pwm = 30; 02626 motor[TIRE_FL].dir = FOR; 02627 motor[TIRE_BL].pwm = 30; 02628 motor[TIRE_BL].dir = FOR; 02629 break; 02630 } 02631 if(g[3]==0&&g[2]==0) { 02632 mode=25; 02633 } 02634 } else if(mode==25) { 02635 motor[TIRE_FR].pwm = 100; 02636 motor[TIRE_FR].dir = BRAKE; 02637 motor[TIRE_FL].pwm = 100; 02638 motor[TIRE_FL].dir = BRAKE; 02639 motor[TIRE_BR].pwm = 100; 02640 motor[TIRE_BR].dir = BRAKE; 02641 motor[TIRE_BL].pwm = 100; 02642 motor[TIRE_BL].dir = BRAKE; 02643 if(UPLS==true||DOLS==true) { 02644 motor[MOTOR_0].pwm = 200; 02645 motor[MOTOR_0].dir = BRAKE; 02646 mode=26; 02647 } else { 02648 motor[MOTOR_0].pwm = 100; 02649 motor[MOTOR_0].dir = BACK; 02650 } 02651 } else if(mode==26) { 02652 switch(g[4]) { 02653 case 98: 02654 motor[TIRE_FR].pwm = 60; 02655 motor[TIRE_FR].dir = FOR; 02656 motor[TIRE_FL].pwm = 60; 02657 motor[TIRE_FL].dir = BACK; 02658 motor[TIRE_BR].pwm = 60; 02659 motor[TIRE_BR].dir = FOR; 02660 motor[TIRE_BL].pwm = 60; 02661 motor[TIRE_BL].dir = BACK; 02662 break; 02663 case 99: 02664 motor[TIRE_FR].pwm = 0; 02665 motor[TIRE_FR].dir = FREE; 02666 motor[TIRE_FL].pwm = 0; 02667 motor[TIRE_FL].dir = FREE; 02668 motor[TIRE_BR].pwm = 0; 02669 motor[TIRE_BR].dir = FREE; 02670 motor[TIRE_BL].pwm = 0; 02671 motor[TIRE_BL].dir = FREE; 02672 break; 02673 case 0: 02674 motor[TIRE_FR].pwm = 60; 02675 motor[TIRE_FR].dir = FOR; 02676 motor[TIRE_FL].pwm = 60; 02677 motor[TIRE_FL].dir = BACK; 02678 motor[TIRE_BR].pwm = 60; 02679 motor[TIRE_BR].dir = FOR; 02680 motor[TIRE_BL].pwm = 60; 02681 motor[TIRE_BL].dir = BACK; 02682 break; 02683 case 255: 02684 motor[TIRE_FR].pwm = 0; 02685 motor[TIRE_FR].dir = FREE; 02686 motor[TIRE_FL].pwm = 0; 02687 motor[TIRE_FL].dir = FREE; 02688 motor[TIRE_BR].pwm = 0; 02689 motor[TIRE_BR].dir = FREE; 02690 motor[TIRE_BL].pwm = 0; 02691 motor[TIRE_BL].dir = FREE; 02692 break; 02693 case 253: 02694 motor[TIRE_FR].pwm = 25; 02695 motor[TIRE_FR].dir = FOR; 02696 motor[TIRE_FL].pwm = 60; 02697 motor[TIRE_FL].dir = BACK; 02698 motor[TIRE_BR].pwm = 60; 02699 motor[TIRE_BR].dir = FOR; 02700 motor[TIRE_BL].pwm = 25; 02701 motor[TIRE_BL].dir = BACK; 02702 break; 02703 case 254: 02704 motor[TIRE_FR].pwm = 40; 02705 motor[TIRE_FR].dir = FOR; 02706 motor[TIRE_FL].pwm = 60; 02707 motor[TIRE_FL].dir = BACK; 02708 motor[TIRE_BR].pwm = 60; 02709 motor[TIRE_BR].dir = FOR; 02710 motor[TIRE_BL].pwm = 40; 02711 motor[TIRE_BL].dir = BACK; 02712 break; 02713 case 1: 02714 motor[TIRE_FR].pwm = 0; 02715 motor[TIRE_FR].dir = FREE; 02716 motor[TIRE_FL].pwm = 0; 02717 motor[TIRE_FL].dir = FREE; 02718 motor[TIRE_BR].pwm = 0; 02719 motor[TIRE_BR].dir = FREE; 02720 motor[TIRE_BL].pwm = 0; 02721 motor[TIRE_BL].dir = FREE; 02722 break; 02723 case 3: 02724 motor[TIRE_FR].pwm = 60; 02725 motor[TIRE_FR].dir = FOR; 02726 motor[TIRE_FL].pwm = 25; 02727 motor[TIRE_FL].dir = BACK; 02728 motor[TIRE_BR].pwm = 25; 02729 motor[TIRE_BR].dir = FOR; 02730 motor[TIRE_BL].pwm = 60; 02731 motor[TIRE_BL].dir = BACK; 02732 break; 02733 case 2: 02734 motor[TIRE_FR].pwm = 60; 02735 motor[TIRE_FR].dir = FOR; 02736 motor[TIRE_FL].pwm = 40; 02737 motor[TIRE_FL].dir = BACK; 02738 motor[TIRE_BR].pwm = 40; 02739 motor[TIRE_BR].dir = FOR; 02740 motor[TIRE_BL].pwm = 60; 02741 motor[TIRE_BL].dir = BACK; 02742 break; 02743 } 02744 if(g[1]==98) { 02745 mode=31; 02746 } 02747 } 02748 } else { 02749 if(mode==21) { 02750 motor[TIRE_FR].pwm = 30; 02751 motor[TIRE_FR].dir = FOR; 02752 motor[TIRE_FL].pwm = 255; 02753 motor[TIRE_FL].dir = BRAKE; 02754 motor[TIRE_BR].pwm = 255; 02755 motor[TIRE_BR].dir = BRAKE; 02756 motor[TIRE_BL].pwm = 30; 02757 motor[TIRE_BL].dir = BACK; 02758 if(g[1]==0) { 02759 mode=22; 02760 fast=60; 02761 normal=40; 02762 slow=25; 02763 } 02764 } else if(mode==22) { 02765 if(g[1]==98) { 02766 slow=25; 02767 normal=30; 02768 fast=40; 02769 } 02770 if(g[3]==0) { 02771 slow=25; 02772 normal=40; 02773 fast=60; 02774 mode=23; 02775 } 02776 switch(g[1]) { 02777 case 98: 02778 solenoid.solenoid4 = SOLENOID_OFF; 02779 fast=40; 02780 normal=30; 02781 slow=25; 02782 motor[TIRE_FR].pwm = normal; 02783 motor[TIRE_FR].dir = FOR; 02784 motor[TIRE_FL].pwm = normal; 02785 motor[TIRE_FL].dir = BACK; 02786 motor[TIRE_BR].pwm = normal; 02787 motor[TIRE_BR].dir = FOR; 02788 motor[TIRE_BL].pwm = normal; 02789 motor[TIRE_BL].dir = BACK; 02790 break; 02791 case 0: 02792 motor[TIRE_FR].pwm = fast; 02793 motor[TIRE_FR].dir = FOR; 02794 motor[TIRE_FL].pwm = fast; 02795 motor[TIRE_FL].dir = BACK; 02796 motor[TIRE_BR].pwm = fast; 02797 motor[TIRE_BR].dir = FOR; 02798 motor[TIRE_BL].pwm = fast; 02799 motor[TIRE_BL].dir = BACK; 02800 mtc=true; 02801 break; 02802 case 255: 02803 motor[TIRE_FR].pwm = fast; 02804 motor[TIRE_FR].dir = FOR; 02805 motor[TIRE_FL].pwm = slow; 02806 motor[TIRE_FL].dir = BACK; 02807 motor[TIRE_BR].pwm = slow; 02808 motor[TIRE_BR].dir = FOR; 02809 motor[TIRE_BL].pwm = fast; 02810 motor[TIRE_BL].dir = BACK; 02811 mtc=true; 02812 break; 02813 case 253: 02814 motor[TIRE_FR].pwm = slow; 02815 motor[TIRE_FR].dir = FOR; 02816 motor[TIRE_FL].pwm = 0; 02817 motor[TIRE_FL].dir = FREE; 02818 motor[TIRE_BR].pwm = 0; 02819 motor[TIRE_BR].dir = FREE; 02820 motor[TIRE_BL].pwm = slow; 02821 motor[TIRE_BL].dir = BACK; 02822 mtc=true; 02823 break; 02824 case 254: 02825 motor[TIRE_FR].pwm = normal; 02826 motor[TIRE_FR].dir = FOR; 02827 motor[TIRE_FL].pwm = 0; 02828 motor[TIRE_FL].dir = FREE; 02829 motor[TIRE_BR].pwm = 0; 02830 motor[TIRE_BR].dir = FREE; 02831 motor[TIRE_BL].pwm = normal; 02832 motor[TIRE_BL].dir = BACK; 02833 mtc=true; 02834 break; 02835 case 1: 02836 motor[TIRE_FR].pwm = slow; 02837 motor[TIRE_FR].dir = FOR; 02838 motor[TIRE_FL].pwm = fast; 02839 motor[TIRE_FL].dir = BACK; 02840 motor[TIRE_BR].pwm = fast; 02841 motor[TIRE_BR].dir = FOR; 02842 motor[TIRE_BL].pwm = slow; 02843 motor[TIRE_BL].dir = BACK; 02844 mtc=true; 02845 break; 02846 case 3: 02847 motor[TIRE_FR].pwm = 0; 02848 motor[TIRE_FR].dir = FREE; 02849 motor[TIRE_FL].pwm = slow; 02850 motor[TIRE_FL].dir = BACK; 02851 motor[TIRE_BR].pwm = slow; 02852 motor[TIRE_BR].dir = FOR; 02853 motor[TIRE_BL].pwm = 0; 02854 motor[TIRE_BL].dir = FREE; 02855 mtc=true; 02856 break; 02857 case 2: 02858 motor[TIRE_FR].pwm = 0; 02859 motor[TIRE_FR].dir = FREE; 02860 motor[TIRE_FL].pwm = normal; 02861 motor[TIRE_FL].dir = BACK; 02862 motor[TIRE_BR].pwm = normal; 02863 motor[TIRE_BR].dir = FOR; 02864 motor[TIRE_BL].pwm = 0; 02865 motor[TIRE_BL].dir = FREE; 02866 mtc=true; 02867 break; 02868 } 02869 if(g[1]!=98&&mtc==true) { 02870 switch(g[0]) { 02871 case 0: 02872 mtc=false; 02873 break; 02874 case 255: 02875 motor[TIRE_FL].pwm += 3; 02876 mtc=false; 02877 break; 02878 case 253: 02879 motor[TIRE_FL].pwm += 6; 02880 mtc=false; 02881 break; 02882 case 254: 02883 motor[TIRE_FL].pwm += 9; 02884 motor[TIRE_FR].pwm = 0; 02885 mtc=false; 02886 break; 02887 case 1: 02888 motor[TIRE_FR].pwm += 3; 02889 mtc=false; 02890 break; 02891 case 3: 02892 motor[TIRE_FR].pwm += 6; 02893 mtc=false; 02894 break; 02895 case 2: 02896 motor[TIRE_FR].pwm += 9; 02897 motor[TIRE_FL].pwm = 0; 02898 mtc=false; 02899 break; 02900 } 02901 } 02902 } else if(mode==23) { 02903 if(UPLS||DOLS) { 02904 if(UPLS||DOLS) { 02905 motor[MOTOR_0].pwm = 200; 02906 motor[MOTOR_0].dir = BRAKE; 02907 fast=60; 02908 normal=40; 02909 slow=25; 02910 countss=0; 02911 mode=24; 02912 } 02913 } else { 02914 motor[MOTOR_0].pwm = 100; 02915 motor[MOTOR_0].dir = BACK; 02916 } 02917 motor[TIRE_FR].pwm = 100; 02918 motor[TIRE_FR].dir = BRAKE; 02919 motor[TIRE_FL].pwm = 100; 02920 motor[TIRE_FL].dir = BRAKE; 02921 motor[TIRE_BR].pwm = 100; 02922 motor[TIRE_BR].dir = BRAKE; 02923 motor[TIRE_BL].pwm = 100; 02924 motor[TIRE_BL].dir = BRAKE; 02925 } else if(mode==24) { 02926 if(g[1]==98&&countss>=40000) { 02927 mode=31; 02928 countss=0; 02929 } 02930 switch(g[1]) { 02931 case 98: 02932 fast=40; 02933 normal=30; 02934 slow=25; 02935 motor[TIRE_FR].pwm = normal; 02936 motor[TIRE_FR].dir = FOR; 02937 motor[TIRE_FL].pwm = normal; 02938 motor[TIRE_FL].dir = BACK; 02939 motor[TIRE_BR].pwm = normal; 02940 motor[TIRE_BR].dir = FOR; 02941 motor[TIRE_BL].pwm = normal; 02942 motor[TIRE_BL].dir = BACK; 02943 break; 02944 case 0: 02945 motor[TIRE_FR].pwm = fast; 02946 motor[TIRE_FR].dir = FOR; 02947 motor[TIRE_FL].pwm = fast; 02948 motor[TIRE_FL].dir = BACK; 02949 motor[TIRE_BR].pwm = fast; 02950 motor[TIRE_BR].dir = FOR; 02951 motor[TIRE_BL].pwm = fast; 02952 motor[TIRE_BL].dir = BACK; 02953 mtc=true; 02954 break; 02955 case 255: 02956 motor[TIRE_FR].pwm = fast; 02957 motor[TIRE_FR].dir = FOR; 02958 motor[TIRE_FL].pwm = slow; 02959 motor[TIRE_FL].dir = BACK; 02960 motor[TIRE_BR].pwm = slow; 02961 motor[TIRE_BR].dir = FOR; 02962 motor[TIRE_BL].pwm = fast; 02963 motor[TIRE_BL].dir = BACK; 02964 mtc=true; 02965 break; 02966 case 253: 02967 motor[TIRE_FR].pwm = slow; 02968 motor[TIRE_FR].dir = FOR; 02969 motor[TIRE_FL].pwm = 0; 02970 motor[TIRE_FL].dir = FREE; 02971 motor[TIRE_BR].pwm = 0; 02972 motor[TIRE_BR].dir = FREE; 02973 motor[TIRE_BL].pwm = slow; 02974 motor[TIRE_BL].dir = BACK; 02975 mtc=true; 02976 break; 02977 case 254: 02978 motor[TIRE_FR].pwm = normal; 02979 motor[TIRE_FR].dir = FOR; 02980 motor[TIRE_FL].pwm = 0; 02981 motor[TIRE_FL].dir = FREE; 02982 motor[TIRE_BR].pwm = 0; 02983 motor[TIRE_BR].dir = FREE; 02984 motor[TIRE_BL].pwm = normal; 02985 motor[TIRE_BL].dir = BACK; 02986 mtc=true; 02987 break; 02988 case 1: 02989 motor[TIRE_FR].pwm = slow; 02990 motor[TIRE_FR].dir = FOR; 02991 motor[TIRE_FL].pwm = fast; 02992 motor[TIRE_FL].dir = BACK; 02993 motor[TIRE_BR].pwm = fast; 02994 motor[TIRE_BR].dir = FOR; 02995 motor[TIRE_BL].pwm = slow; 02996 motor[TIRE_BL].dir = BACK; 02997 mtc=true; 02998 break; 02999 case 3: 03000 motor[TIRE_FR].pwm = 0; 03001 motor[TIRE_FR].dir = FREE; 03002 motor[TIRE_FL].pwm = slow; 03003 motor[TIRE_FL].dir = BACK; 03004 motor[TIRE_BR].pwm = slow; 03005 motor[TIRE_BR].dir = FOR; 03006 motor[TIRE_BL].pwm = 0; 03007 motor[TIRE_BL].dir = FREE; 03008 mtc=true; 03009 break; 03010 case 2: 03011 motor[TIRE_FR].pwm = 0; 03012 motor[TIRE_FR].dir = FREE; 03013 motor[TIRE_FL].pwm = normal; 03014 motor[TIRE_FL].dir = BACK; 03015 motor[TIRE_BR].pwm = normal; 03016 motor[TIRE_BR].dir = FOR; 03017 motor[TIRE_BL].pwm = 0; 03018 motor[TIRE_BL].dir = FREE; 03019 mtc=true; 03020 break; 03021 } 03022 if(g[1]!=98&&mtc==true) { 03023 switch(g[0]) { 03024 case 0: 03025 mtc=false; 03026 break; 03027 case 255: 03028 motor[TIRE_FL].pwm += 3; 03029 mtc=false; 03030 break; 03031 case 253: 03032 motor[TIRE_FL].pwm += 6; 03033 mtc=false; 03034 break; 03035 case 254: 03036 motor[TIRE_FL].pwm += 9; 03037 motor[TIRE_FR].pwm = 0; 03038 mtc=false; 03039 break; 03040 case 1: 03041 motor[TIRE_FR].pwm += 3; 03042 mtc=false; 03043 break; 03044 case 3: 03045 motor[TIRE_FR].pwm += 6; 03046 mtc=false; 03047 break; 03048 case 2: 03049 motor[TIRE_FR].pwm += 9; 03050 motor[TIRE_FL].pwm = 0; 03051 mtc=false; 03052 break; 03053 } 03054 } 03055 } 03056 } 03057 03058 /* 03059 if(mode==31) { 03060 motor[TIRE_FR].pwm = 0; 03061 motor[TIRE_FR].dir = FREE; 03062 motor[TIRE_FL].pwm = 0; 03063 motor[TIRE_FL].dir = FREE; 03064 motor[TIRE_BR].pwm = 0; 03065 motor[TIRE_BR].dir = FREE; 03066 motor[TIRE_BL].pwm = 0; 03067 motor[TIRE_BL].dir = FREE; 03068 if(countss>=5000) { 03069 countss=0; 03070 mode=32; 03071 } 03072 } else if(mode==32) { 03073 motor[TIRE_FR].pwm = 10; 03074 motor[TIRE_FR].dir = BRAKE; 03075 motor[TIRE_FL].pwm = 10; 03076 motor[TIRE_FL].dir = BRAKE; 03077 motor[TIRE_BR].pwm = 10; 03078 motor[TIRE_BR].dir = BRAKE; 03079 motor[TIRE_BL].pwm = 10; 03080 motor[TIRE_BL].dir = BRAKE; 03081 if(countss>=40000) { 03082 mode=33; 03083 countss=0; 03084 } 03085 } 03086 */ 03087 if(mode==31) { 03088 motor[TIRE_FR].pwm = 255; 03089 motor[TIRE_FR].dir = BRAKE; 03090 motor[TIRE_FL].pwm = 45; 03091 motor[TIRE_FL].dir = BACK; 03092 motor[TIRE_BR].pwm = 45; 03093 motor[TIRE_BR].dir = FOR; 03094 motor[TIRE_BL].pwm = 255; 03095 motor[TIRE_BL].dir = BRAKE; 03096 if(g[2]==0) { 03097 cross=0; 03098 countss=0; 03099 mode=40; 03100 } 03101 } else if(mode==40) { 03102 switch(g[3]) { 03103 case 98: 03104 motor[TIRE_FR].pwm = normal+15; 03105 motor[TIRE_FR].dir = BACK; 03106 motor[TIRE_FL].pwm = normal+15; 03107 motor[TIRE_FL].dir = BACK; 03108 motor[TIRE_BR].pwm = normal+15; 03109 motor[TIRE_BR].dir = FOR; 03110 motor[TIRE_BL].pwm = normal+15; 03111 motor[TIRE_BL].dir = FOR; 03112 if(countss>30000) { 03113 cross++; 03114 countss=0; 03115 } 03116 mtc2=true; 03117 break; 03118 case 0: 03119 motor[TIRE_FR].pwm = fast+15; 03120 motor[TIRE_FR].dir = BACK; 03121 motor[TIRE_FL].pwm = fast+15; 03122 motor[TIRE_FL].dir = BACK; 03123 motor[TIRE_BR].pwm = fast+15; 03124 motor[TIRE_BR].dir = FOR; 03125 motor[TIRE_BL].pwm = fast+15; 03126 motor[TIRE_BL].dir = FOR; 03127 mtc2=true; 03128 break; 03129 case 255: 03130 motor[TIRE_FR].pwm = slow+15; 03131 motor[TIRE_FR].dir = BACK; 03132 motor[TIRE_FL].pwm = fast+15; 03133 motor[TIRE_FL].dir = BACK; 03134 motor[TIRE_BR].pwm = fast+15; 03135 motor[TIRE_BR].dir = FOR; 03136 motor[TIRE_BL].pwm = slow+15; 03137 motor[TIRE_BL].dir = FOR; 03138 mtc2=true; 03139 break; 03140 case 253: 03141 motor[TIRE_FR].pwm = 0; 03142 motor[TIRE_FR].dir = FREE; 03143 motor[TIRE_FL].pwm = slow+15; 03144 motor[TIRE_FL].dir = BACK; 03145 motor[TIRE_BR].pwm = slow+15; 03146 motor[TIRE_BR].dir = FOR; 03147 motor[TIRE_BL].pwm = 0; 03148 motor[TIRE_BL].dir = FREE; 03149 mtc2=true; 03150 break; 03151 case 254: 03152 motor[TIRE_FR].pwm = 0; 03153 motor[TIRE_FR].dir = FREE; 03154 motor[TIRE_FL].pwm = normal+15; 03155 motor[TIRE_FL].dir = BACK; 03156 motor[TIRE_BR].pwm = normal+15; 03157 motor[TIRE_BR].dir = FOR; 03158 motor[TIRE_BL].pwm = 0; 03159 motor[TIRE_BL].dir = FREE; 03160 mtc2=true; 03161 break; 03162 case 1: 03163 motor[TIRE_FR].pwm = fast+15; 03164 motor[TIRE_FR].dir = BACK; 03165 motor[TIRE_FL].pwm = slow+15; 03166 motor[TIRE_FL].dir = BACK; 03167 motor[TIRE_BR].pwm = slow+15; 03168 motor[TIRE_BR].dir = FOR; 03169 motor[TIRE_BL].pwm = fast+15; 03170 motor[TIRE_BL].dir = FOR; 03171 mtc2=true; 03172 break; 03173 case 3: 03174 motor[TIRE_FR].pwm = slow+15; 03175 motor[TIRE_FR].dir = BACK; 03176 motor[TIRE_FL].pwm = 0; 03177 motor[TIRE_FL].dir = FREE; 03178 motor[TIRE_BR].pwm = 0; 03179 motor[TIRE_BR].dir = FREE; 03180 motor[TIRE_BL].pwm = slow+15; 03181 motor[TIRE_BL].dir = FOR; 03182 mtc2=true; 03183 break; 03184 case 2: 03185 motor[TIRE_FR].pwm = normal+15; 03186 motor[TIRE_FR].dir = BACK; 03187 motor[TIRE_FL].pwm = 0; 03188 motor[TIRE_FL].dir = FREE; 03189 motor[TIRE_BR].pwm = 0; 03190 motor[TIRE_BR].dir = FREE; 03191 motor[TIRE_BL].pwm = normal+15; 03192 motor[TIRE_BL].dir = FOR; 03193 mtc2=true; 03194 break; 03195 } 03196 if(g[2]!=98&&mtc2==true) { 03197 switch(g[2]) { 03198 case 0: 03199 mtc2=false; 03200 break; 03201 case 255: 03202 motor[TIRE_FR].pwm += 3; 03203 mtc2=false; 03204 break; 03205 case 253: 03206 motor[TIRE_FR].pwm += 6; 03207 mtc2=false; 03208 break; 03209 case 254: 03210 motor[TIRE_FR].pwm += 9; 03211 motor[TIRE_BR].pwm = 0; 03212 mtc2=false; 03213 break; 03214 case 1: 03215 motor[TIRE_BR].pwm += 3; 03216 mtc2=false; 03217 break; 03218 case 3: 03219 motor[TIRE_BR].pwm += 6; 03220 mtc2=false; 03221 break; 03222 case 2: 03223 motor[TIRE_BR].pwm += 9; 03224 motor[TIRE_FR].pwm = 0; 03225 mtc2=false; 03226 break; 03227 } 03228 } 03229 03230 if(YELLOW) { 03231 if(cross==1) { 03232 mode=41; 03233 countss=0; 03234 cross=0; 03235 } 03236 } else { 03237 if(cross==3) { 03238 mode=41; 03239 countss=0; 03240 cross=0; 03241 } 03242 } 03243 if(LIF||LIB) { 03244 mode=42; 03245 } 03246 03247 } else if(mode==41) { 03248 if(LIF||LIB) { 03249 mode=42; 03250 } 03251 motor[TIRE_FR].pwm = 255; 03252 motor[TIRE_FR].dir = BRAKE; 03253 motor[TIRE_FL].pwm = 40; 03254 motor[TIRE_FL].dir = BACK; 03255 motor[TIRE_BR].pwm = 40; 03256 motor[TIRE_BR].dir = FOR; 03257 motor[TIRE_BL].pwm = 255; 03258 motor[TIRE_BL].dir = BRAKE; 03259 if(g[1]==0) { 03260 countss=0; 03261 mode=70; 03262 normal=80; 03263 fast=100; 03264 slow=60; 03265 } 03266 } else if(mode==42) { 03267 motor[TIRE_FR].pwm = 40; 03268 motor[TIRE_FR].dir = FOR; 03269 motor[TIRE_FL].pwm = 0; 03270 motor[TIRE_FL].dir = FREE; 03271 motor[TIRE_BR].pwm = 0; 03272 motor[TIRE_BR].dir = FREE; 03273 motor[TIRE_BL].pwm = 40; 03274 motor[TIRE_BL].dir = BACK; 03275 if(g[1]==0) { 03276 countss=0; 03277 mode=70; 03278 } 03279 } else if(mode==70) { 03280 switch(g[1]) { 03281 case 99: 03282 if(countss>=100000) { 03283 mode=100; 03284 } 03285 break; 03286 case 98: 03287 motor[TIRE_FR].pwm = normal; 03288 motor[TIRE_FR].dir = FOR; 03289 motor[TIRE_FL].pwm = normal; 03290 motor[TIRE_FL].dir = BACK; 03291 motor[TIRE_BR].pwm = normal+5; 03292 motor[TIRE_BR].dir = FOR; 03293 motor[TIRE_BL].pwm = normal; 03294 motor[TIRE_BL].dir = BACK; 03295 break; 03296 case 0: 03297 motor[TIRE_FR].pwm = 100; 03298 motor[TIRE_FR].dir = FOR; 03299 motor[TIRE_FL].pwm = 100; 03300 motor[TIRE_FL].dir = BACK; 03301 motor[TIRE_BR].pwm = 105; 03302 motor[TIRE_BR].dir = FOR; 03303 motor[TIRE_BL].pwm = 100; 03304 motor[TIRE_BL].dir = BACK; 03305 mtc=true; 03306 break; 03307 case 255: 03308 motor[TIRE_FR].pwm = fast; 03309 motor[TIRE_FR].dir = FOR; 03310 motor[TIRE_FL].pwm = slow; 03311 motor[TIRE_FL].dir = BACK; 03312 motor[TIRE_BR].pwm = slow+5; 03313 motor[TIRE_BR].dir = FOR; 03314 motor[TIRE_BL].pwm = fast; 03315 motor[TIRE_BL].dir = BACK; 03316 mtc=true; 03317 break; 03318 case 253: 03319 motor[TIRE_FR].pwm = slow; 03320 motor[TIRE_FR].dir = FOR; 03321 motor[TIRE_FL].pwm = 0; 03322 motor[TIRE_FL].dir = FREE; 03323 motor[TIRE_BR].pwm = 0; 03324 motor[TIRE_BR].dir = FREE; 03325 motor[TIRE_BL].pwm = slow; 03326 motor[TIRE_BL].dir = BACK; 03327 mtc=true; 03328 break; 03329 case 254: 03330 motor[TIRE_FR].pwm = normal; 03331 motor[TIRE_FR].dir = FOR; 03332 motor[TIRE_FL].pwm = 0; 03333 motor[TIRE_FL].dir = FREE; 03334 motor[TIRE_BR].pwm = 0; 03335 motor[TIRE_BR].dir = FREE; 03336 motor[TIRE_BL].pwm = normal; 03337 motor[TIRE_BL].dir = BACK; 03338 mtc=true; 03339 break; 03340 case 1: 03341 motor[TIRE_FR].pwm = slow; 03342 motor[TIRE_FR].dir = FOR; 03343 motor[TIRE_FL].pwm = fast; 03344 motor[TIRE_FL].dir = BACK; 03345 motor[TIRE_BR].pwm = fast+5; 03346 motor[TIRE_BR].dir = FOR; 03347 motor[TIRE_BL].pwm = slow; 03348 motor[TIRE_BL].dir = BACK; 03349 mtc=true; 03350 break; 03351 case 3: 03352 motor[TIRE_FR].pwm = 0; 03353 motor[TIRE_FR].dir = FREE; 03354 motor[TIRE_FL].pwm = slow; 03355 motor[TIRE_FL].dir = BACK; 03356 motor[TIRE_BR].pwm = slow+5; 03357 motor[TIRE_BR].dir = FOR; 03358 motor[TIRE_BL].pwm = 0; 03359 motor[TIRE_BL].dir = FREE; 03360 mtc=true; 03361 break; 03362 case 2: 03363 motor[TIRE_FR].pwm = 0; 03364 motor[TIRE_FR].dir = FREE; 03365 motor[TIRE_FL].pwm = normal; 03366 motor[TIRE_FL].dir = BACK; 03367 motor[TIRE_BR].pwm = normal+5; 03368 motor[TIRE_BR].dir = FOR; 03369 motor[TIRE_BL].pwm = 0; 03370 motor[TIRE_BL].dir = FREE; 03371 mtc=true; 03372 break; 03373 } 03374 if(g[1]!=98&&mtc==true) { 03375 switch(g[0]) { 03376 case 0: 03377 mtc=false; 03378 break; 03379 case 255: 03380 motor[TIRE_FL].pwm += 3; 03381 mtc=false; 03382 break; 03383 case 253: 03384 motor[TIRE_FL].pwm += 6; 03385 mtc=false; 03386 break; 03387 case 254: 03388 motor[TIRE_FL].pwm += 9; 03389 motor[TIRE_FR].pwm = 0; 03390 mtc=false; 03391 break; 03392 case 1: 03393 motor[TIRE_FR].pwm += 3; 03394 mtc=false; 03395 break; 03396 case 3: 03397 motor[TIRE_FR].pwm += 6; 03398 mtc=false; 03399 break; 03400 case 2: 03401 motor[TIRE_FR].pwm += 9; 03402 motor[TIRE_FL].pwm = 0; 03403 mtc=false; 03404 break; 03405 } 03406 } 03407 } else if(mode==100) { 03408 motor[TIRE_FR].pwm = 50; 03409 motor[TIRE_FR].dir = FOR; 03410 motor[TIRE_FL].pwm = 55; 03411 motor[TIRE_FL].dir = BACK; 03412 motor[TIRE_BR].pwm = 60; 03413 motor[TIRE_BR].dir = FOR; 03414 motor[TIRE_BL].pwm = 50; 03415 motor[TIRE_BL].dir = BACK; 03416 } 03417 03418 } 03419 #endif 03420 03421 #if USE_PROCESS_NUM>4 03422 static void Process4() 03423 { 03424 //Oh So 決勝青ゾーン 03425 LED::LED::TapeledMode(2); 03426 countss++; 03427 if(mode==1) {//スタートゾーンから白線検知 03428 fast=40; 03429 normal=30; 03430 slow=20; 03431 motor[TIRE_FR].pwm = normal+20; 03432 motor[TIRE_FR].dir = FOR; 03433 motor[TIRE_FL].pwm = normal+20; 03434 motor[TIRE_FL].dir = BACK; 03435 motor[TIRE_BR].pwm = normal+20; 03436 motor[TIRE_BR].dir = FOR; 03437 motor[TIRE_BL].pwm = normal+20; 03438 motor[TIRE_BL].dir = BACK; 03439 if(g[2]==0&&countss>=100000) { 03440 countss=100000; 03441 cross=0; 03442 mode=3; 03443 } 03444 } else if(mode==3) { //横ライントレースから縦ライントレースへ 03445 motor[TIRE_FR].pwm = 30; 03446 motor[TIRE_FR].dir = FOR; 03447 motor[TIRE_FL].pwm = 255; 03448 motor[TIRE_FL].dir = BRAKE; 03449 motor[TIRE_BR].pwm = 255; 03450 motor[TIRE_BR].dir = BRAKE; 03451 motor[TIRE_BL].pwm = 30; 03452 motor[TIRE_BL].dir = BACK; 03453 if(g[1]==0) { 03454 mode=10; 03455 countss=100000; 03456 } 03457 } else if(mode==10) { //縦ライントレース 03458 switch(g[1]) { 03459 case 98: 03460 motor[TIRE_FR].pwm = normal+50; 03461 motor[TIRE_FR].dir = FOR; 03462 motor[TIRE_FL].pwm = normal+50; 03463 motor[TIRE_FL].dir = BACK; 03464 motor[TIRE_BR].pwm = normal+50; 03465 motor[TIRE_BR].dir = FOR; 03466 motor[TIRE_BL].pwm = normal+50; 03467 motor[TIRE_BL].dir = BACK; 03468 mtc=true; 03469 if(YELLOW) { 03470 if(countss>80000) { 03471 cross++; 03472 countss=0; 03473 } 03474 03475 if(cross==2) { 03476 fast = 10; 03477 normal = 10; 03478 slow = 5; 03479 } 03480 03481 if(cross==3) { 03482 fast=40; 03483 normal=30; 03484 slow=20; 03485 mode=11; 03486 countss=0; 03487 cross=0; 03488 } 03489 } else { 03490 mode=11; 03491 } 03492 break; 03493 case 0: 03494 motor[TIRE_FR].pwm = fast+50; 03495 motor[TIRE_FR].dir = FOR; 03496 motor[TIRE_FL].pwm = fast+50; 03497 motor[TIRE_FL].dir = BACK; 03498 motor[TIRE_BR].pwm = fast+50; 03499 motor[TIRE_BR].dir = FOR; 03500 motor[TIRE_BL].pwm = fast+50; 03501 motor[TIRE_BL].dir = BACK; 03502 mtc=true; 03503 break; 03504 case 255: 03505 motor[TIRE_FR].pwm = fast+50; 03506 motor[TIRE_FR].dir = FOR; 03507 motor[TIRE_FL].pwm = slow+50; 03508 motor[TIRE_FL].dir = BACK; 03509 motor[TIRE_BR].pwm = slow+50; 03510 motor[TIRE_BR].dir = FOR; 03511 motor[TIRE_BL].pwm = fast+50; 03512 motor[TIRE_BL].dir = BACK; 03513 mtc=true; 03514 break; 03515 case 253: 03516 motor[TIRE_FR].pwm = slow+50; 03517 motor[TIRE_FR].dir = FOR; 03518 motor[TIRE_FL].pwm = 0; 03519 motor[TIRE_FL].dir = FREE; 03520 motor[TIRE_BR].pwm = 0; 03521 motor[TIRE_BR].dir = FREE; 03522 motor[TIRE_BL].pwm = slow+50; 03523 motor[TIRE_BL].dir = BACK; 03524 mtc=true; 03525 break; 03526 case 254: 03527 motor[TIRE_FR].pwm = normal+50; 03528 motor[TIRE_FR].dir = FOR; 03529 motor[TIRE_FL].pwm = 0; 03530 motor[TIRE_FL].dir = FREE; 03531 motor[TIRE_BR].pwm = 0; 03532 motor[TIRE_BR].dir = FREE; 03533 motor[TIRE_BL].pwm = normal+50; 03534 motor[TIRE_BL].dir = BACK; 03535 mtc=true; 03536 break; 03537 case 1: 03538 motor[TIRE_FR].pwm = slow+50; 03539 motor[TIRE_FR].dir = FOR; 03540 motor[TIRE_FL].pwm = fast+50; 03541 motor[TIRE_FL].dir = BACK; 03542 motor[TIRE_BR].pwm = fast+50; 03543 motor[TIRE_BR].dir = FOR; 03544 motor[TIRE_BL].pwm = slow+50; 03545 motor[TIRE_BL].dir = BACK; 03546 mtc=true; 03547 break; 03548 case 3: 03549 motor[TIRE_FR].pwm = 0; 03550 motor[TIRE_FR].dir = FREE; 03551 motor[TIRE_FL].pwm = slow+50; 03552 motor[TIRE_FL].dir = BACK; 03553 motor[TIRE_BR].pwm = slow+50; 03554 motor[TIRE_BR].dir = FOR; 03555 motor[TIRE_BL].pwm = 0; 03556 motor[TIRE_BL].dir = FREE; 03557 mtc=true; 03558 break; 03559 case 2: 03560 motor[TIRE_FR].pwm = 0; 03561 motor[TIRE_FR].dir = FREE; 03562 motor[TIRE_FL].pwm = normal+50; 03563 motor[TIRE_FL].dir = BACK; 03564 motor[TIRE_BR].pwm = normal+50; 03565 motor[TIRE_BR].dir = FOR; 03566 motor[TIRE_BL].pwm = 0; 03567 motor[TIRE_BL].dir = FREE; 03568 mtc=true; 03569 break; 03570 } 03571 if(g[1]!=98&&mtc==true) { 03572 switch(g[0]) { 03573 case 0: 03574 mtc=false; 03575 break; 03576 case 255: 03577 motor[TIRE_FL].pwm += 3; 03578 mtc=false; 03579 break; 03580 case 253: 03581 motor[TIRE_FL].pwm += 6; 03582 mtc=false; 03583 break; 03584 case 254: 03585 motor[TIRE_FL].pwm += 9; 03586 motor[TIRE_FR].pwm = 0; 03587 mtc=false; 03588 break; 03589 case 1: 03590 motor[TIRE_FR].pwm += 3; 03591 mtc=false; 03592 break; 03593 case 3: 03594 motor[TIRE_FR].pwm += 6; 03595 mtc=false; 03596 break; 03597 case 2: 03598 motor[TIRE_FR].pwm += 9; 03599 motor[TIRE_FL].pwm = 0; 03600 mtc=false; 03601 break; 03602 } 03603 } 03604 } else if(mode==11) { 03605 motor[TIRE_FR].pwm = normal; 03606 motor[TIRE_FR].dir = FOR; 03607 motor[TIRE_FL].pwm = 50; 03608 motor[TIRE_FL].dir = BRAKE; 03609 motor[TIRE_BR].pwm = 50; 03610 motor[TIRE_BR].dir = BRAKE; 03611 motor[TIRE_BL].pwm = normal; 03612 motor[TIRE_BL].dir = BACK; 03613 if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { 03614 countss=0; 03615 cross=0; 03616 mode=20; 03617 } 03618 } else if(mode==20) { 03619 switch(g[2]) { 03620 case 98: 03621 motor[TIRE_FR].pwm = normal+20; 03622 motor[TIRE_FR].dir = FOR; 03623 motor[TIRE_FL].pwm = normal+20; 03624 motor[TIRE_FL].dir = FOR; 03625 motor[TIRE_BR].pwm = normal+20; 03626 motor[TIRE_BR].dir = BACK; 03627 motor[TIRE_BL].pwm = normal+20; 03628 motor[TIRE_BL].dir = BACK; 03629 if(countss>30000) { 03630 cross++; 03631 countss=0; 03632 } 03633 mtc2=true; 03634 03635 if(YELLOW) { 03636 solenoid.solenoid4= SOLENOID_ON; 03637 if(cross==1) { 03638 fast=20; 03639 normal=15; 03640 slow=15; 03641 } else if(cross==2) { 03642 mode=21; 03643 countss=0; 03644 cross=0; 03645 } 03646 } else { 03647 if(cross==2) { 03648 fast=20; 03649 normal=15; 03650 slow=15; 03651 } 03652 if(cross==3) { 03653 mode=21; 03654 countss=0; 03655 cross=0; 03656 } 03657 } 03658 break; 03659 case 0: 03660 motor[TIRE_FR].pwm = fast+20; 03661 motor[TIRE_FR].dir = FOR; 03662 motor[TIRE_FL].pwm = fast+20; 03663 motor[TIRE_FL].dir = FOR; 03664 motor[TIRE_BR].pwm = fast+20; 03665 motor[TIRE_BR].dir = BACK; 03666 motor[TIRE_BL].pwm = fast+20; 03667 motor[TIRE_BL].dir = BACK; 03668 mtc2=true; 03669 break; 03670 case 255: 03671 motor[TIRE_FR].pwm = normal+20; 03672 motor[TIRE_FR].dir = FOR; 03673 motor[TIRE_FL].pwm = fast+20; 03674 motor[TIRE_FL].dir = FOR; 03675 motor[TIRE_BR].pwm = fast+20; 03676 motor[TIRE_BR].dir = BACK; 03677 motor[TIRE_BL].pwm = normal+20; 03678 motor[TIRE_BL].dir = BACK; 03679 mtc2=true; 03680 break; 03681 case 253: 03682 motor[TIRE_FR].pwm = 0; 03683 motor[TIRE_FR].dir = FREE; 03684 motor[TIRE_FL].pwm = slow+20; 03685 motor[TIRE_FL].dir = FOR; 03686 motor[TIRE_BR].pwm = slow+20; 03687 motor[TIRE_BR].dir = BACK; 03688 motor[TIRE_BL].pwm = 0; 03689 motor[TIRE_BL].dir = FREE; 03690 mtc2=true; 03691 break; 03692 case 254: 03693 motor[TIRE_FR].pwm = 0; 03694 motor[TIRE_FR].dir = FREE; 03695 motor[TIRE_FL].pwm = normal+20; 03696 motor[TIRE_FL].dir = FOR; 03697 motor[TIRE_BR].pwm = normal+20; 03698 motor[TIRE_BR].dir = BACK; 03699 motor[TIRE_BL].pwm = 0; 03700 motor[TIRE_BL].dir = FREE; 03701 mtc2=true; 03702 break; 03703 case 1: 03704 motor[TIRE_FR].pwm = fast+20; 03705 motor[TIRE_FR].dir = FOR; 03706 motor[TIRE_FL].pwm = slow+20; 03707 motor[TIRE_FL].dir = FOR; 03708 motor[TIRE_BR].pwm = slow+20; 03709 motor[TIRE_BR].dir = BACK; 03710 motor[TIRE_BL].pwm = fast+20; 03711 motor[TIRE_BL].dir = BACK; 03712 mtc2=true; 03713 break; 03714 case 3: 03715 motor[TIRE_FR].pwm = slow+20; 03716 motor[TIRE_FR].dir = FOR; 03717 motor[TIRE_FL].pwm = 0; 03718 motor[TIRE_FL].dir = FREE; 03719 motor[TIRE_BR].pwm = 0; 03720 motor[TIRE_BR].dir = FREE; 03721 motor[TIRE_BL].pwm = slow+20; 03722 motor[TIRE_BL].dir = BACK; 03723 mtc2=true; 03724 break; 03725 case 2: 03726 motor[TIRE_FR].pwm = normal+20; 03727 motor[TIRE_FR].dir = FOR; 03728 motor[TIRE_FL].pwm = 0; 03729 motor[TIRE_FL].dir = FREE; 03730 motor[TIRE_BR].pwm = 0; 03731 motor[TIRE_BR].dir = FREE; 03732 motor[TIRE_BL].pwm = normal+20; 03733 motor[TIRE_BL].dir = BACK; 03734 mtc2=true; 03735 break; 03736 } 03737 if(g[2]!=98&&mtc2==true) { 03738 switch(g[3]) { 03739 case 0: 03740 mtc2=false; 03741 break; 03742 case 255: 03743 motor[TIRE_BL].pwm += 5; 03744 mtc2=false; 03745 break; 03746 case 253: 03747 motor[TIRE_BL].pwm += 10; 03748 mtc2=false; 03749 break; 03750 case 254: 03751 motor[TIRE_BL].pwm += 15; 03752 motor[TIRE_FL].pwm = 0; 03753 mtc2=false; 03754 break; 03755 case 1: 03756 motor[TIRE_FL].pwm += 5; 03757 mtc2=false; 03758 break; 03759 case 3: 03760 motor[TIRE_FL].pwm += 10; 03761 mtc2=false; 03762 break; 03763 case 2: 03764 motor[TIRE_FL].pwm += 15; 03765 motor[TIRE_BL].pwm = 0; 03766 mtc2=false; 03767 break; 03768 } 03769 } 03770 } 03771 if(YELLOW) { 03772 if(mode==21) { 03773 motor[TIRE_FR].pwm = 20; 03774 motor[TIRE_FR].dir = BACK; 03775 motor[TIRE_FL].pwm = 20; 03776 motor[TIRE_FL].dir = BACK; 03777 motor[TIRE_BR].pwm = 20; 03778 motor[TIRE_BR].dir = FOR; 03779 motor[TIRE_BL].pwm = 20; 03780 motor[TIRE_BL].dir = FOR; 03781 if(g[3]==98&&countss>=10000) { 03782 mode=22; 03783 countss=0; 03784 fast=60; 03785 normal=40; 03786 slow=20; 03787 } 03788 } else if(mode==22) { 03789 if(g[4]==3||g[4]==2) { 03790 motor[TIRE_FR].pwm = 30; 03791 motor[TIRE_FR].dir = FOR; 03792 motor[TIRE_FL].pwm = 30; 03793 motor[TIRE_FL].dir = FOR; 03794 motor[TIRE_BR].pwm = 30; 03795 motor[TIRE_BR].dir = BACK; 03796 motor[TIRE_BL].pwm = 30; 03797 motor[TIRE_BL].dir = BACK; 03798 } 03799 if(g[4]==253||g[4]==254) { 03800 motor[TIRE_FR].pwm = 30; 03801 motor[TIRE_FR].dir = BACK; 03802 motor[TIRE_FL].pwm = 30; 03803 motor[TIRE_FL].dir = BACK; 03804 motor[TIRE_BR].pwm = 30; 03805 motor[TIRE_BR].dir = FOR; 03806 motor[TIRE_BL].pwm = 30; 03807 motor[TIRE_BL].dir = FOR; 03808 } 03809 switch(g[2]) { 03810 case 0: 03811 motor[TIRE_FR].pwm = 20; 03812 motor[TIRE_FR].dir = FREE; 03813 motor[TIRE_BR].pwm = 20; 03814 motor[TIRE_BR].dir = FREE; 03815 break; 03816 case 255: 03817 motor[TIRE_FR].pwm = 30; 03818 motor[TIRE_FR].dir = BACK; 03819 motor[TIRE_BR].pwm = 20; 03820 motor[TIRE_BR].dir = FREE; 03821 break; 03822 case 253: 03823 motor[TIRE_FR].pwm = 40; 03824 motor[TIRE_FR].dir = BACK; 03825 motor[TIRE_BR].pwm = 20; 03826 motor[TIRE_BR].dir = FREE; 03827 break; 03828 case 254: 03829 motor[TIRE_FR].pwm = 30; 03830 motor[TIRE_FR].dir = BACK; 03831 motor[TIRE_BR].pwm = 30; 03832 motor[TIRE_BR].dir = BACK; 03833 break; 03834 case 1: 03835 motor[TIRE_FR].pwm = 20; 03836 motor[TIRE_FR].dir = FREE; 03837 motor[TIRE_BR].pwm = 30; 03838 motor[TIRE_BR].dir = FOR; 03839 break; 03840 case 3: 03841 motor[TIRE_FR].pwm = 20; 03842 motor[TIRE_FR].dir = FREE; 03843 motor[TIRE_BR].pwm = 40; 03844 motor[TIRE_BR].dir = FOR; 03845 break; 03846 case 2: 03847 motor[TIRE_FR].pwm = 30; 03848 motor[TIRE_FR].dir = FOR; 03849 motor[TIRE_BR].pwm = 30; 03850 motor[TIRE_BR].dir = FOR; 03851 break; 03852 } 03853 switch(g[3]) { 03854 case 0: 03855 motor[TIRE_FL].pwm = 20; 03856 motor[TIRE_FL].dir = FREE; 03857 motor[TIRE_BL].pwm = 20; 03858 motor[TIRE_BL].dir = FREE; 03859 break; 03860 case 255: 03861 motor[TIRE_FL].pwm = 20; 03862 motor[TIRE_FL].dir = FREE; 03863 motor[TIRE_BL].pwm = 30; 03864 motor[TIRE_BL].dir = BACK; 03865 case 253: 03866 motor[TIRE_FL].pwm = 20; 03867 motor[TIRE_FL].dir = FREE; 03868 motor[TIRE_BL].pwm = 40; 03869 motor[TIRE_BL].dir = BACK; 03870 break; 03871 case 254: 03872 motor[TIRE_FL].pwm = 30; 03873 motor[TIRE_FL].dir = BACK; 03874 motor[TIRE_BL].pwm = 30; 03875 motor[TIRE_BL].dir = BACK; 03876 break; 03877 case 1: 03878 motor[TIRE_FL].pwm = 30; 03879 motor[TIRE_FL].dir = FOR; 03880 motor[TIRE_BL].pwm = 20; 03881 motor[TIRE_BL].dir = FREE; 03882 break; 03883 case 3: 03884 motor[TIRE_FL].pwm = 40; 03885 motor[TIRE_FL].dir = FOR; 03886 motor[TIRE_BL].pwm = 20; 03887 motor[TIRE_BL].dir = FREE; 03888 break; 03889 case 2: 03890 motor[TIRE_FL].pwm = 30; 03891 motor[TIRE_FL].dir = FOR; 03892 motor[TIRE_BL].pwm = 30; 03893 motor[TIRE_BL].dir = FOR; 03894 break; 03895 } 03896 if(g[3]==0&&g[2]==0) { 03897 mode=23; 03898 countss=0; 03899 } 03900 } else if(mode==23) { 03901 switch(g[4]) { 03902 case 98: 03903 motor[TIRE_FR].pwm = fast-5; 03904 motor[TIRE_FR].dir = BACK; 03905 motor[TIRE_FL].pwm = fast-5; 03906 motor[TIRE_FL].dir = FOR; 03907 motor[TIRE_BR].pwm = fast-5; 03908 motor[TIRE_BR].dir = BACK; 03909 motor[TIRE_BL].pwm = fast-5; 03910 motor[TIRE_BL].dir = FOR; 03911 break; 03912 case 99: 03913 motor[TIRE_FR].pwm = 0; 03914 motor[TIRE_FR].dir = FREE; 03915 motor[TIRE_FL].pwm = 0; 03916 motor[TIRE_FL].dir = FREE; 03917 motor[TIRE_BR].pwm = 0; 03918 motor[TIRE_BR].dir = FREE; 03919 motor[TIRE_BL].pwm = 0; 03920 motor[TIRE_BL].dir = FREE; 03921 break; 03922 case 0: 03923 motor[TIRE_FR].pwm = fast-5; 03924 motor[TIRE_FR].dir = BACK; 03925 motor[TIRE_FL].pwm = fast-5; 03926 motor[TIRE_FL].dir = FOR; 03927 motor[TIRE_BR].pwm = fast-5; 03928 motor[TIRE_BR].dir = BACK; 03929 motor[TIRE_BL].pwm = fast-5; 03930 motor[TIRE_BL].dir = FOR; 03931 break; 03932 case 255: 03933 motor[TIRE_FR].pwm = 0; 03934 motor[TIRE_FR].dir = FREE; 03935 motor[TIRE_FL].pwm = 0; 03936 motor[TIRE_FL].dir = FREE; 03937 motor[TIRE_BR].pwm = 0; 03938 motor[TIRE_BR].dir = FREE; 03939 motor[TIRE_BL].pwm = 0; 03940 motor[TIRE_BL].dir = FREE; 03941 break; 03942 case 253: 03943 motor[TIRE_FR].pwm = fast-5; 03944 motor[TIRE_FR].dir = BACK; 03945 motor[TIRE_FL].pwm = slow-5; 03946 motor[TIRE_FL].dir = FOR; 03947 motor[TIRE_BR].pwm = slow-5; 03948 motor[TIRE_BR].dir = BACK; 03949 motor[TIRE_BL].pwm = fast-5; 03950 motor[TIRE_BL].dir = FOR; 03951 break; 03952 case 254: 03953 motor[TIRE_FR].pwm = fast-5; 03954 motor[TIRE_FR].dir = BACK; 03955 motor[TIRE_FL].pwm = normal-5; 03956 motor[TIRE_FL].dir = FOR; 03957 motor[TIRE_BR].pwm = normal-5; 03958 motor[TIRE_BR].dir = BACK; 03959 motor[TIRE_BL].pwm = fast-5; 03960 motor[TIRE_BL].dir = FOR; 03961 break; 03962 case 1: 03963 motor[TIRE_FR].pwm = 0; 03964 motor[TIRE_FR].dir = FREE; 03965 motor[TIRE_FL].pwm = 0; 03966 motor[TIRE_FL].dir = FREE; 03967 motor[TIRE_BR].pwm = 0; 03968 motor[TIRE_BR].dir = FREE; 03969 motor[TIRE_BL].pwm = 0; 03970 motor[TIRE_BL].dir = FREE; 03971 break; 03972 case 3: 03973 motor[TIRE_FR].pwm = slow-5; 03974 motor[TIRE_FR].dir = BACK; 03975 motor[TIRE_FL].pwm = fast-5; 03976 motor[TIRE_FL].dir = FOR; 03977 motor[TIRE_BR].pwm = fast-5; 03978 motor[TIRE_BR].dir = BACK; 03979 motor[TIRE_BL].pwm = slow-5; 03980 motor[TIRE_BL].dir = FOR; 03981 break; 03982 case 2: 03983 motor[TIRE_FR].pwm = normal-5; 03984 motor[TIRE_FR].dir = BACK; 03985 motor[TIRE_FL].pwm = fast-5; 03986 motor[TIRE_FL].dir = FOR; 03987 motor[TIRE_BR].pwm = fast-5; 03988 motor[TIRE_BR].dir = BACK; 03989 motor[TIRE_BL].pwm = normal-5; 03990 motor[TIRE_BL].dir = FOR; 03991 break; 03992 } 03993 if(g[2]==254&&countss>=100000) { 03994 mode=24; 03995 solenoid.solenoid4 = SOLENOID_OFF; 03996 countss=0; 03997 fast=60; 03998 normal=40; 03999 slow=20; 04000 } 04001 } else if(mode==24) { 04002 motor[TIRE_FR].pwm = 60; 04003 motor[TIRE_FR].dir = BACK; 04004 motor[TIRE_FL].pwm = 60; 04005 motor[TIRE_FL].dir = BRAKE; 04006 motor[TIRE_BR].pwm = 60; 04007 motor[TIRE_BR].dir = BRAKE; 04008 motor[TIRE_BL].pwm = 60; 04009 motor[TIRE_BL].dir = FOR; 04010 if(g[0]==0) { 04011 mode=25; 04012 } 04013 } else if(mode==25) { 04014 motor[TIRE_FR].pwm = 100; 04015 motor[TIRE_FR].dir = BRAKE; 04016 motor[TIRE_FL].pwm = 100; 04017 motor[TIRE_FL].dir = BRAKE; 04018 motor[TIRE_BR].pwm = 100; 04019 motor[TIRE_BR].dir = BRAKE; 04020 motor[TIRE_BL].pwm = 100; 04021 motor[TIRE_BL].dir = BRAKE; 04022 if(DOLS==true||UPLS==true) { 04023 motor[MOTOR_0].pwm = 200; 04024 motor[MOTOR_0].dir = BRAKE; 04025 mode=26; 04026 countss=0; 04027 } else { 04028 motor[MOTOR_0].pwm = 100; 04029 motor[MOTOR_0].dir = BACK; 04030 } 04031 } else if(mode==26) { 04032 if(g[0]==98&&countss>=40000) { 04033 mode=31; 04034 countss=0; 04035 } 04036 switch(g[0]) { 04037 case 98: 04038 fast=40; 04039 normal=30; 04040 slow=25; 04041 motor[TIRE_FR].pwm = normal; 04042 motor[TIRE_FR].dir = BACK; 04043 motor[TIRE_FL].pwm = normal; 04044 motor[TIRE_FL].dir = FOR; 04045 motor[TIRE_BR].pwm = normal; 04046 motor[TIRE_BR].dir = BACK; 04047 motor[TIRE_BL].pwm = normal; 04048 motor[TIRE_BL].dir = FOR; 04049 break; 04050 case 0: 04051 motor[TIRE_FR].pwm = fast; 04052 motor[TIRE_FR].dir = BACK; 04053 motor[TIRE_FL].pwm = fast; 04054 motor[TIRE_FL].dir = FOR; 04055 motor[TIRE_BR].pwm = fast; 04056 motor[TIRE_BR].dir = BACK; 04057 motor[TIRE_BL].pwm = fast; 04058 motor[TIRE_BL].dir = FOR; 04059 mtc=true; 04060 break; 04061 case 255: 04062 motor[TIRE_FR].pwm = fast; 04063 motor[TIRE_FR].dir = BACK; 04064 motor[TIRE_FL].pwm = slow; 04065 motor[TIRE_FL].dir = FOR; 04066 motor[TIRE_BR].pwm = slow; 04067 motor[TIRE_BR].dir = BACK; 04068 motor[TIRE_BL].pwm = fast; 04069 motor[TIRE_BL].dir = FOR; 04070 mtc=true; 04071 break; 04072 case 253: 04073 motor[TIRE_FR].pwm = slow; 04074 motor[TIRE_FR].dir = BACK; 04075 motor[TIRE_FL].pwm = 0; 04076 motor[TIRE_FL].dir = FREE; 04077 motor[TIRE_BR].pwm = 0; 04078 motor[TIRE_BR].dir = FREE; 04079 motor[TIRE_BL].pwm = slow; 04080 motor[TIRE_BL].dir = FOR; 04081 mtc=true; 04082 break; 04083 case 254: 04084 motor[TIRE_FR].pwm = normal; 04085 motor[TIRE_FR].dir = BACK; 04086 motor[TIRE_FL].pwm = 0; 04087 motor[TIRE_FL].dir = FREE; 04088 motor[TIRE_BR].pwm = 0; 04089 motor[TIRE_BR].dir = FREE; 04090 motor[TIRE_BL].pwm = normal; 04091 motor[TIRE_BL].dir = FOR; 04092 mtc=true; 04093 break; 04094 case 1: 04095 motor[TIRE_FR].pwm = slow; 04096 motor[TIRE_FR].dir = BACK; 04097 motor[TIRE_FL].pwm = fast; 04098 motor[TIRE_FL].dir = FOR; 04099 motor[TIRE_BR].pwm = fast; 04100 motor[TIRE_BR].dir = BACK; 04101 motor[TIRE_BL].pwm = slow; 04102 motor[TIRE_BL].dir = FOR; 04103 mtc=true; 04104 break; 04105 case 3: 04106 motor[TIRE_FR].pwm = 0; 04107 motor[TIRE_FR].dir = FREE; 04108 motor[TIRE_FL].pwm = slow; 04109 motor[TIRE_FL].dir = FOR; 04110 motor[TIRE_BR].pwm = slow; 04111 motor[TIRE_BR].dir = BACK; 04112 motor[TIRE_BL].pwm = 0; 04113 motor[TIRE_BL].dir = FREE; 04114 mtc=true; 04115 break; 04116 case 2: 04117 motor[TIRE_FR].pwm = 0; 04118 motor[TIRE_FR].dir = FREE; 04119 motor[TIRE_FL].pwm = normal; 04120 motor[TIRE_FL].dir = FOR; 04121 motor[TIRE_BR].pwm = normal; 04122 motor[TIRE_BR].dir = BACK; 04123 motor[TIRE_BL].pwm = 0; 04124 motor[TIRE_BL].dir = FREE; 04125 mtc=true; 04126 break; 04127 } 04128 if(g[1]!=98&&mtc==true) { 04129 switch(g[1]) { 04130 case 0: 04131 mtc=false; 04132 break; 04133 case 255: 04134 motor[TIRE_BR].pwm += 5; 04135 mtc=false; 04136 break; 04137 case 253: 04138 motor[TIRE_BR].pwm += 10; 04139 mtc=false; 04140 break; 04141 case 254: 04142 motor[TIRE_BR].pwm += 20; 04143 motor[TIRE_BL].pwm = 0; 04144 mtc=false; 04145 break; 04146 case 1: 04147 motor[TIRE_BL].pwm += 5; 04148 mtc=false; 04149 break; 04150 case 3: 04151 motor[TIRE_BL].pwm += 10; 04152 mtc=false; 04153 break; 04154 case 2: 04155 motor[TIRE_BL].pwm += 20; 04156 motor[TIRE_BR].pwm = 0; 04157 mtc=false; 04158 break; 04159 } 04160 } 04161 } 04162 } else { 04163 if(mode==21) { 04164 if(g[0]==98&&countss>=40000) { 04165 fast=60; 04166 normal=30; 04167 slow=25; 04168 mode=31; 04169 countss=0; 04170 } 04171 switch(g[0]) { 04172 case 98: 04173 fast=40; 04174 normal=30; 04175 slow=25; 04176 motor[TIRE_FR].pwm = normal; 04177 motor[TIRE_FR].dir = BACK; 04178 motor[TIRE_FL].pwm = normal; 04179 motor[TIRE_FL].dir = FOR; 04180 motor[TIRE_BR].pwm = normal; 04181 motor[TIRE_BR].dir = BACK; 04182 motor[TIRE_BL].pwm = normal; 04183 motor[TIRE_BL].dir = FOR; 04184 break; 04185 case 0: 04186 motor[TIRE_FR].pwm = fast; 04187 motor[TIRE_FR].dir = BACK; 04188 motor[TIRE_FL].pwm = fast; 04189 motor[TIRE_FL].dir = FOR; 04190 motor[TIRE_BR].pwm = fast; 04191 motor[TIRE_BR].dir = BACK; 04192 motor[TIRE_BL].pwm = fast; 04193 motor[TIRE_BL].dir = FOR; 04194 mtc=true; 04195 break; 04196 case 255: 04197 motor[TIRE_FR].pwm = fast; 04198 motor[TIRE_FR].dir = BACK; 04199 motor[TIRE_FL].pwm = slow; 04200 motor[TIRE_FL].dir = FOR; 04201 motor[TIRE_BR].pwm = slow; 04202 motor[TIRE_BR].dir = BACK; 04203 motor[TIRE_BL].pwm = fast; 04204 motor[TIRE_BL].dir = FOR; 04205 mtc=true; 04206 break; 04207 case 253: 04208 motor[TIRE_FR].pwm = slow; 04209 motor[TIRE_FR].dir = BACK; 04210 motor[TIRE_FL].pwm = 0; 04211 motor[TIRE_FL].dir = FREE; 04212 motor[TIRE_BR].pwm = 0; 04213 motor[TIRE_BR].dir = FREE; 04214 motor[TIRE_BL].pwm = slow; 04215 motor[TIRE_BL].dir = FOR; 04216 mtc=true; 04217 break; 04218 case 254: 04219 motor[TIRE_FR].pwm = normal; 04220 motor[TIRE_FR].dir = BACK; 04221 motor[TIRE_FL].pwm = 0; 04222 motor[TIRE_FL].dir = FREE; 04223 motor[TIRE_BR].pwm = 0; 04224 motor[TIRE_BR].dir = FREE; 04225 motor[TIRE_BL].pwm = normal; 04226 motor[TIRE_BL].dir = FOR; 04227 mtc=true; 04228 break; 04229 case 1: 04230 motor[TIRE_FR].pwm = slow; 04231 motor[TIRE_FR].dir = BACK; 04232 motor[TIRE_FL].pwm = fast; 04233 motor[TIRE_FL].dir = FOR; 04234 motor[TIRE_BR].pwm = fast; 04235 motor[TIRE_BR].dir = BACK; 04236 motor[TIRE_BL].pwm = slow; 04237 motor[TIRE_BL].dir = FOR; 04238 mtc=true; 04239 break; 04240 case 3: 04241 motor[TIRE_FR].pwm = 0; 04242 motor[TIRE_FR].dir = FREE; 04243 motor[TIRE_FL].pwm = slow; 04244 motor[TIRE_FL].dir = FOR; 04245 motor[TIRE_BR].pwm = slow; 04246 motor[TIRE_BR].dir = BACK; 04247 motor[TIRE_BL].pwm = 0; 04248 motor[TIRE_BL].dir = FREE; 04249 mtc=true; 04250 break; 04251 case 2: 04252 motor[TIRE_FR].pwm = 0; 04253 motor[TIRE_FR].dir = FREE; 04254 motor[TIRE_FL].pwm = normal; 04255 motor[TIRE_FL].dir = FOR; 04256 motor[TIRE_BR].pwm = normal; 04257 motor[TIRE_BR].dir = BACK; 04258 motor[TIRE_BL].pwm = 0; 04259 motor[TIRE_BL].dir = FREE; 04260 mtc=true; 04261 break; 04262 } 04263 if(g[1]!=98&&mtc==true) { 04264 switch(g[1]) { 04265 case 0: 04266 mtc=false; 04267 break; 04268 case 255: 04269 motor[TIRE_BR].pwm += 5; 04270 mtc=false; 04271 break; 04272 case 253: 04273 motor[TIRE_BR].pwm += 10; 04274 mtc=false; 04275 break; 04276 case 254: 04277 motor[TIRE_BR].pwm += 20; 04278 motor[TIRE_BL].pwm = 0; 04279 mtc=false; 04280 break; 04281 case 1: 04282 motor[TIRE_BL].pwm += 5; 04283 mtc=false; 04284 break; 04285 case 3: 04286 motor[TIRE_BL].pwm += 10; 04287 mtc=false; 04288 break; 04289 case 2: 04290 motor[TIRE_BL].pwm += 20; 04291 motor[TIRE_BR].pwm = 0; 04292 mtc=false; 04293 break; 04294 } 04295 } 04296 } 04297 } 04298 04299 /* 04300 if(mode==31) { 04301 motor[TIRE_FR].pwm = 0; 04302 motor[TIRE_FR].dir = FREE; 04303 motor[TIRE_FL].pwm = 0; 04304 motor[TIRE_FL].dir = FREE; 04305 motor[TIRE_BR].pwm = 0; 04306 motor[TIRE_BR].dir = FREE; 04307 motor[TIRE_BL].pwm = 0; 04308 motor[TIRE_BL].dir = FREE; 04309 if(countss>=5000) { 04310 countss=0; 04311 mode=33; 04312 } 04313 } else if(mode==33) { 04314 motor[TIRE_FR].pwm = 30; 04315 motor[TIRE_FR].dir = BACK; 04316 motor[TIRE_FL].pwm = 0; 04317 motor[TIRE_FL].dir = FREE; 04318 motor[TIRE_BR].pwm = 0; 04319 motor[TIRE_BR].dir = FREE; 04320 motor[TIRE_BL].pwm = 30; 04321 motor[TIRE_BL].dir = FOR; 04322 if(g[3]==0) { 04323 cross=0; 04324 countss=0; 04325 mode=40; 04326 } 04327 } else 04328 */ 04329 if(mode==31) { 04330 motor[TIRE_FR].pwm = 45; 04331 motor[TIRE_FR].dir = BACK; 04332 motor[TIRE_FL].pwm = 255; 04333 motor[TIRE_FL].dir = BRAKE; 04334 motor[TIRE_BR].pwm = 255; 04335 motor[TIRE_BR].dir = BRAKE; 04336 motor[TIRE_BL].pwm = 45; 04337 motor[TIRE_BL].dir = FOR; 04338 if(g[3]==0) { 04339 cross=0; 04340 countss=0; 04341 mode=40; 04342 } 04343 } else if(mode==40) { 04344 switch(g[3]) { 04345 case 98: 04346 motor[TIRE_FR].pwm = normal+15; 04347 motor[TIRE_FR].dir = BACK; 04348 motor[TIRE_FL].pwm = normal+15; 04349 motor[TIRE_FL].dir = BACK; 04350 motor[TIRE_BR].pwm = normal+15; 04351 motor[TIRE_BR].dir = FOR; 04352 motor[TIRE_BL].pwm = normal+15; 04353 motor[TIRE_BL].dir = FOR; 04354 if(countss>10000) { 04355 cross++; 04356 countss=0; 04357 } 04358 mtc2=true; 04359 if(cross==1) { 04360 mode=41; 04361 countss=0; 04362 cross=0; 04363 } 04364 break; 04365 case 0: 04366 motor[TIRE_FR].pwm = fast+15; 04367 motor[TIRE_FR].dir = BACK; 04368 motor[TIRE_FL].pwm = fast+15; 04369 motor[TIRE_FL].dir = BACK; 04370 motor[TIRE_BR].pwm = fast+15; 04371 motor[TIRE_BR].dir = FOR; 04372 motor[TIRE_BL].pwm = fast+15; 04373 motor[TIRE_BL].dir = FOR; 04374 mtc2=true; 04375 break; 04376 case 255: 04377 motor[TIRE_FR].pwm = slow+15; 04378 motor[TIRE_FR].dir = BACK; 04379 motor[TIRE_FL].pwm = fast+15; 04380 motor[TIRE_FL].dir = BACK; 04381 motor[TIRE_BR].pwm = fast+15; 04382 motor[TIRE_BR].dir = FOR; 04383 motor[TIRE_BL].pwm = slow+15; 04384 motor[TIRE_BL].dir = FOR; 04385 mtc2=true; 04386 break; 04387 case 253: 04388 motor[TIRE_FR].pwm = 0; 04389 motor[TIRE_FR].dir = FREE; 04390 motor[TIRE_FL].pwm = slow+15; 04391 motor[TIRE_FL].dir = BACK; 04392 motor[TIRE_BR].pwm = slow+15; 04393 motor[TIRE_BR].dir = FOR; 04394 motor[TIRE_BL].pwm = 0; 04395 motor[TIRE_BL].dir = FREE; 04396 mtc2=true; 04397 break; 04398 case 254: 04399 motor[TIRE_FR].pwm = 0; 04400 motor[TIRE_FR].dir = FREE; 04401 motor[TIRE_FL].pwm = normal+15; 04402 motor[TIRE_FL].dir = BACK; 04403 motor[TIRE_BR].pwm = normal+15; 04404 motor[TIRE_BR].dir = FOR; 04405 motor[TIRE_BL].pwm = 0; 04406 motor[TIRE_BL].dir = FREE; 04407 mtc2=true; 04408 break; 04409 case 1: 04410 motor[TIRE_FR].pwm = fast+15; 04411 motor[TIRE_FR].dir = BACK; 04412 motor[TIRE_FL].pwm = slow+15; 04413 motor[TIRE_FL].dir = BACK; 04414 motor[TIRE_BR].pwm = slow+15; 04415 motor[TIRE_BR].dir = FOR; 04416 motor[TIRE_BL].pwm = fast+15; 04417 motor[TIRE_BL].dir = FOR; 04418 mtc2=true; 04419 break; 04420 case 3: 04421 motor[TIRE_FR].pwm = slow+15; 04422 motor[TIRE_FR].dir = BACK; 04423 motor[TIRE_FL].pwm = 0; 04424 motor[TIRE_FL].dir = FREE; 04425 motor[TIRE_BR].pwm = 0; 04426 motor[TIRE_BR].dir = FREE; 04427 motor[TIRE_BL].pwm = slow+15; 04428 motor[TIRE_BL].dir = FOR; 04429 mtc2=true; 04430 break; 04431 case 2: 04432 motor[TIRE_FR].pwm = normal+15; 04433 motor[TIRE_FR].dir = BACK; 04434 motor[TIRE_FL].pwm = 0; 04435 motor[TIRE_FL].dir = FREE; 04436 motor[TIRE_BR].pwm = 0; 04437 motor[TIRE_BR].dir = FREE; 04438 motor[TIRE_BL].pwm = normal+15; 04439 motor[TIRE_BL].dir = FOR; 04440 mtc2=true; 04441 break; 04442 } 04443 if(g[2]!=98&&mtc2==true) { 04444 switch(g[2]) { 04445 case 0: 04446 mtc2=false; 04447 break; 04448 case 255: 04449 motor[TIRE_FL].pwm += 5; 04450 mtc2=false; 04451 break; 04452 case 253: 04453 motor[TIRE_FL].pwm += 10; 04454 mtc2=false; 04455 break; 04456 case 254: 04457 motor[TIRE_FL].pwm += 20; 04458 motor[TIRE_BL].pwm = 0; 04459 mtc2=false; 04460 break; 04461 case 1: 04462 motor[TIRE_BL].pwm += 5; 04463 mtc2=false; 04464 break; 04465 case 3: 04466 motor[TIRE_BL].pwm += 10; 04467 mtc2=false; 04468 break; 04469 case 2: 04470 motor[TIRE_BL].pwm += 20; 04471 motor[TIRE_FL].pwm = 0; 04472 mtc2=false; 04473 break; 04474 } 04475 } 04476 } else if(mode==41) { 04477 if(LIF||LIB) { 04478 mode=42; 04479 } 04480 motor[TIRE_FR].pwm = 30; 04481 motor[TIRE_FR].dir = BACK; 04482 motor[TIRE_FL].pwm = 0; 04483 motor[TIRE_FL].dir = FREE; 04484 motor[TIRE_BR].pwm = 0; 04485 motor[TIRE_BR].dir = FREE; 04486 motor[TIRE_BL].pwm = 30; 04487 motor[TIRE_BL].dir = FOR; 04488 if(g[1]==0) { 04489 countss=0; 04490 mode=70; 04491 normal=60; 04492 fast=70; 04493 slow=40; 04494 } 04495 } else if(mode==42) { 04496 motor[TIRE_FR].pwm = 40; 04497 motor[TIRE_FR].dir = FOR; 04498 motor[TIRE_FL].pwm = 0; 04499 motor[TIRE_FL].dir = FREE; 04500 motor[TIRE_BR].pwm = 0; 04501 motor[TIRE_BR].dir = FREE; 04502 motor[TIRE_BL].pwm = 40; 04503 motor[TIRE_BL].dir = BACK; 04504 if(g[1]==0) { 04505 countss=0; 04506 mode=70; 04507 } 04508 } else if(mode==70) { 04509 switch(g[0]) { 04510 case 99: 04511 if(countss>=100000) { 04512 mode=100; 04513 } 04514 break; 04515 case 98: 04516 motor[TIRE_FR].pwm = normal; 04517 motor[TIRE_FR].dir = BACK; 04518 motor[TIRE_FL].pwm = normal; 04519 motor[TIRE_FL].dir = FOR; 04520 motor[TIRE_BR].pwm = normal; 04521 motor[TIRE_BR].dir = BACK; 04522 motor[TIRE_BL].pwm = normal; 04523 motor[TIRE_BL].dir = FOR; 04524 break; 04525 case 0: 04526 motor[TIRE_FR].pwm = 80; 04527 motor[TIRE_FR].dir = BACK; 04528 motor[TIRE_FL].pwm = 80; 04529 motor[TIRE_FL].dir = FOR; 04530 motor[TIRE_BR].pwm = 80; 04531 motor[TIRE_BR].dir = BACK; 04532 motor[TIRE_BL].pwm = 80; 04533 motor[TIRE_BL].dir = FOR; 04534 mtc=true; 04535 break; 04536 case 255: 04537 motor[TIRE_FR].pwm = fast; 04538 motor[TIRE_FR].dir = BACK; 04539 motor[TIRE_FL].pwm = slow; 04540 motor[TIRE_FL].dir = FOR; 04541 motor[TIRE_BR].pwm = slow; 04542 motor[TIRE_BR].dir = BACK; 04543 motor[TIRE_BL].pwm = fast; 04544 motor[TIRE_BL].dir = FOR; 04545 mtc=true; 04546 break; 04547 case 253: 04548 motor[TIRE_FR].pwm = slow; 04549 motor[TIRE_FR].dir = BACK; 04550 motor[TIRE_FL].pwm = 0; 04551 motor[TIRE_FL].dir = FREE; 04552 motor[TIRE_BR].pwm = 0; 04553 motor[TIRE_BR].dir = FREE; 04554 motor[TIRE_BL].pwm = slow; 04555 motor[TIRE_BL].dir = FOR; 04556 mtc=true; 04557 break; 04558 case 254: 04559 motor[TIRE_FR].pwm = normal; 04560 motor[TIRE_FR].dir = BACK; 04561 motor[TIRE_FL].pwm = 0; 04562 motor[TIRE_FL].dir = FREE; 04563 motor[TIRE_BR].pwm = 0; 04564 motor[TIRE_BR].dir = FREE; 04565 motor[TIRE_BL].pwm = normal; 04566 motor[TIRE_BL].dir = FOR; 04567 mtc=true; 04568 break; 04569 case 1: 04570 motor[TIRE_FR].pwm = slow; 04571 motor[TIRE_FR].dir = BACK; 04572 motor[TIRE_FL].pwm = fast; 04573 motor[TIRE_FL].dir = FOR; 04574 motor[TIRE_BR].pwm = fast; 04575 motor[TIRE_BR].dir = BACK; 04576 motor[TIRE_BL].pwm = slow; 04577 motor[TIRE_BL].dir = FOR; 04578 mtc=true; 04579 break; 04580 case 3: 04581 motor[TIRE_FR].pwm = 0; 04582 motor[TIRE_FR].dir = FREE; 04583 motor[TIRE_FL].pwm = slow; 04584 motor[TIRE_FL].dir = FOR; 04585 motor[TIRE_BR].pwm = slow; 04586 motor[TIRE_BR].dir = BACK; 04587 motor[TIRE_BL].pwm = 0; 04588 motor[TIRE_BL].dir = FREE; 04589 mtc=true; 04590 break; 04591 case 2: 04592 motor[TIRE_FR].pwm = 0; 04593 motor[TIRE_FR].dir = FREE; 04594 motor[TIRE_FL].pwm = normal; 04595 motor[TIRE_FL].dir = FOR; 04596 motor[TIRE_BR].pwm = normal; 04597 motor[TIRE_BR].dir = BACK; 04598 motor[TIRE_BL].pwm = 0; 04599 motor[TIRE_BL].dir = FREE; 04600 mtc=true; 04601 break; 04602 } 04603 if(g[1]!=98&&mtc==true) { 04604 switch(g[1]) { 04605 case 0: 04606 mtc=false; 04607 break; 04608 case 255: 04609 motor[TIRE_BR].pwm += 5; 04610 mtc=false; 04611 break; 04612 case 253: 04613 motor[TIRE_BR].pwm += 10; 04614 mtc=false; 04615 break; 04616 case 254: 04617 motor[TIRE_BR].pwm += 20; 04618 motor[TIRE_BL].pwm = 0; 04619 mtc=false; 04620 break; 04621 case 1: 04622 motor[TIRE_BL].pwm += 5; 04623 mtc=false; 04624 break; 04625 case 3: 04626 motor[TIRE_BL].pwm += 10; 04627 mtc=false; 04628 break; 04629 case 2: 04630 motor[TIRE_BL].pwm += 20; 04631 motor[TIRE_BR].pwm = 0; 04632 mtc=false; 04633 break; 04634 } 04635 } 04636 } else if(mode==100) { 04637 motor[TIRE_FR].pwm = 50; 04638 motor[TIRE_FR].dir = BACK; 04639 motor[TIRE_FL].pwm = 50; 04640 motor[TIRE_FL].dir = FOR; 04641 motor[TIRE_BR].pwm = 50; 04642 motor[TIRE_BR].dir = BACK; 04643 motor[TIRE_BL].pwm = 50; 04644 motor[TIRE_BL].dir = FOR; 04645 } else { 04646 } 04647 04648 } 04649 #endif 04650 04651 #if USE_PROCESS_NUM>5 04652 static void Process5() 04653 { 04654 04655 } 04656 #endif 04657 04658 #if USE_PROCESS_NUM>6 04659 static void Process6() 04660 { 04661 04662 } 04663 #endif 04664 04665 #if USE_PROCESS_NUM>7 04666 static void Process7() 04667 { 04668 04669 } 04670 #endif 04671 04672 #if USE_PROCESS_NUM>8 04673 static void Process8() 04674 { 04675 04676 } 04677 #endif 04678 04679 #if USE_PROCESS_NUM>9 04680 static void Process9() 04681 { 04682 04683 } 04684 #endif 04685 #endif 04686 #pragma endregion PROCESS 04687 04688 static void AllActuatorReset() 04689 { 04690 #ifdef USE_SOLENOID 04691 solenoid.all = ALL_SOLENOID_OFF; 04692 #endif 04693 04694 #ifdef USE_MOTOR 04695 for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { 04696 motor[i].dir = FREE; 04697 motor[i].pwm = 0; 04698 } 04699 #endif 04700 } 04701 04702 #pragma region USER-DEFINED-FUNCTIONS 04703 04704 uint8_t SetStatus(int pwmVal) 04705 { 04706 if (pwmVal < 0) return BACK; 04707 else if (pwmVal > 0) return FOR; 04708 else if (pwmVal == 0) return BRAKE; 04709 else return BRAKE; 04710 } 04711 04712 uint8_t SetPWM(int pwmVal) 04713 { 04714 if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; 04715 else return abs(pwmVal); 04716 } 04717 04718 #pragma endregion
Generated on Sat Jul 30 2022 20:08:28 by
1.7.2