aa

Dependencies:   mbed TrapezoidControl QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

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