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