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