aa

Dependencies:   mbed TrapezoidControl QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

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