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