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         if(g[2]==0&&count>=100000) {
01310             count=100000;
01311             cross=0;
01312             mode=3;
01313         }
01314     } else if(mode==3) { //横ライントレースから縦ライントレースへ
01315         motor[TIRE_FR].pwm = 30;
01316         motor[TIRE_FR].dir = FOR;
01317         motor[TIRE_FL].pwm = 0;
01318         motor[TIRE_FL].dir = FREE;
01319         motor[TIRE_BR].pwm = 0;
01320         motor[TIRE_BR].dir = FREE;
01321         motor[TIRE_BL].pwm = 30;
01322         motor[TIRE_BL].dir = BACK;
01323         if(g[1]==0) {
01324             mode=10;
01325             count=100000;
01326         }
01327     } else if(mode==10) { //縦ライントレース
01328 
01329         if(counts==false&&g[2]==0) {
01330             cros++;
01331             counts=true;
01332         }
01333         if(counts==true&&g[2]==99) {
01334             counts=false;
01335         }
01336 
01337         if(cros==Twsh) {
01338             mode=11;
01339             count=0;
01340             cros=0;
01341         }//ゆっくりモードに入れなかった時の保険
01342         switch(g[1]) {
01343             case 98:
01344                 motor[TIRE_FR].pwm = normal;
01345                 motor[TIRE_FR].dir = FOR;
01346                 motor[TIRE_FL].pwm = normal;
01347                 motor[TIRE_FL].dir = BACK;
01348                 motor[TIRE_BR].pwm = normal;
01349                 motor[TIRE_BR].dir = FOR;
01350                 motor[TIRE_BL].pwm = normal;
01351                 motor[TIRE_BL].dir = BACK;
01352                 mtc=true;
01353                 if(count>80000) {
01354                     cross++;
01355                     count=0;
01356                 }
01357                 if(cross==Twsh) {
01358                     mode=11;
01359                     count=0;
01360                     cross=0;
01361                 }
01362                 break;
01363             case 0:
01364                 motor[TIRE_FR].pwm = fast;
01365                 motor[TIRE_FR].dir = FOR;
01366                 motor[TIRE_FL].pwm = fast;
01367                 motor[TIRE_FL].dir = BACK;
01368                 motor[TIRE_BR].pwm = fast;
01369                 motor[TIRE_BR].dir = FOR;
01370                 motor[TIRE_BL].pwm = fast;
01371                 motor[TIRE_BL].dir = BACK;
01372                 mtc=true;
01373                 break;
01374             case 255:
01375                 motor[TIRE_FR].pwm = fast;
01376                 motor[TIRE_FR].dir = FOR;
01377                 motor[TIRE_FL].pwm = slow;
01378                 motor[TIRE_FL].dir = BACK;
01379                 motor[TIRE_BR].pwm = slow;
01380                 motor[TIRE_BR].dir = FOR;
01381                 motor[TIRE_BL].pwm = fast;
01382                 motor[TIRE_BL].dir = BACK;
01383                 mtc=true;
01384                 break;
01385             case 253:
01386                 motor[TIRE_FR].pwm = slow;
01387                 motor[TIRE_FR].dir = FOR;
01388                 motor[TIRE_FL].pwm = 0;
01389                 motor[TIRE_FL].dir = FREE;
01390                 motor[TIRE_BR].pwm = 0;
01391                 motor[TIRE_BR].dir = FREE;
01392                 motor[TIRE_BL].pwm = slow;
01393                 motor[TIRE_BL].dir = BACK;
01394                 mtc=true;
01395                 break;
01396             case 254:
01397                 motor[TIRE_FR].pwm = normal;
01398                 motor[TIRE_FR].dir = FOR;
01399                 motor[TIRE_FL].pwm = 0;
01400                 motor[TIRE_FL].dir = FREE;
01401                 motor[TIRE_BR].pwm = 0;
01402                 motor[TIRE_BR].dir = FREE;
01403                 motor[TIRE_BL].pwm = normal;
01404                 motor[TIRE_BL].dir = BACK;
01405                 mtc=true;
01406                 break;
01407             case 1:
01408                 motor[TIRE_FR].pwm = slow;
01409                 motor[TIRE_FR].dir = FOR;
01410                 motor[TIRE_FL].pwm = fast;
01411                 motor[TIRE_FL].dir = BACK;
01412                 motor[TIRE_BR].pwm = fast;
01413                 motor[TIRE_BR].dir = FOR;
01414                 motor[TIRE_BL].pwm = slow;
01415                 motor[TIRE_BL].dir = BACK;
01416                 mtc=true;
01417                 break;
01418             case 3:
01419                 motor[TIRE_FR].pwm = 0;
01420                 motor[TIRE_FR].dir = FREE;
01421                 motor[TIRE_FL].pwm = slow;
01422                 motor[TIRE_FL].dir = BACK;
01423                 motor[TIRE_BR].pwm = slow;
01424                 motor[TIRE_BR].dir = FOR;
01425                 motor[TIRE_BL].pwm = 0;
01426                 motor[TIRE_BL].dir = FREE;
01427                 mtc=true;
01428                 break;
01429             case 2:
01430                 motor[TIRE_FR].pwm = 0;
01431                 motor[TIRE_FR].dir = FREE;
01432                 motor[TIRE_FL].pwm = normal;
01433                 motor[TIRE_FL].dir = BACK;
01434                 motor[TIRE_BR].pwm = normal;
01435                 motor[TIRE_BR].dir = FOR;
01436                 motor[TIRE_BL].pwm = 0;
01437                 motor[TIRE_BL].dir = FREE;
01438                 mtc=true;
01439                 break;
01440         }
01441         if(g[0]!=98&&mtc==true) {
01442             switch(g[0]) {
01443                 case 0:
01444                     mtc=false;
01445                     break;
01446                 case 255:
01447                     motor[TIRE_FL].pwm += 5;
01448                     mtc=false;
01449                     break;
01450                 case 253:
01451                     motor[TIRE_FL].pwm += 10;
01452                     mtc=false;
01453                     break;
01454                 case 254:
01455                     motor[TIRE_FL].pwm += 20;
01456                     motor[TIRE_FR].pwm = 0;
01457                     mtc=false;
01458                     break;
01459                 case 1:
01460                     motor[TIRE_FR].pwm += 5;
01461                     mtc=false;
01462                     break;
01463                 case 3:
01464                     motor[TIRE_FR].pwm += 10;
01465                     mtc=false;
01466                     break;
01467                 case 2:
01468                     motor[TIRE_FR].pwm += 20;
01469                     motor[TIRE_FL].pwm = 0;
01470                     mtc=false;
01471                     break;
01472             }
01473         }
01474     } else if(mode==11) {
01475         motor[TIRE_FR].pwm = normal;
01476         motor[TIRE_FR].dir = FOR;
01477         motor[TIRE_FL].pwm = 0;
01478         motor[TIRE_FL].dir = FREE;
01479         motor[TIRE_BR].pwm = 0;
01480         motor[TIRE_BR].dir = FREE;
01481         motor[TIRE_BL].pwm = normal;
01482         motor[TIRE_BL].dir = BACK;
01483         if(g[2]==0) {
01484             count=0;
01485             cross=0;
01486             mode=20;
01487         }
01488     } else if(mode==20) {
01489 
01490         if(Twsh==2) {
01491             solenoid.solenoid2 = SOLENOID_ON;
01492         }
01493 
01494         if(counts==false&&g[1]==0) {
01495             cros++;
01496             counts=true;
01497         }
01498         if(counts==true&&g[1]==99) {
01499             counts=false;
01500         }
01501 
01502         if(cros==2) {
01503             //mode=21;
01504             //count=0;
01505             //cros=0;
01506         }//ゆっくりモードに入れなかった時の保険
01507         if(g[1]==0) {
01508             fast=35;
01509             normal=20;
01510             slow=15;
01511         }
01512         switch(g[2]) {
01513             case 98:
01514                 motor[TIRE_FR].pwm = normal;
01515                 motor[TIRE_FR].dir = FOR;
01516                 motor[TIRE_FL].pwm = normal;
01517                 motor[TIRE_FL].dir = FOR;
01518                 motor[TIRE_BR].pwm = normal;
01519                 motor[TIRE_BR].dir = BACK;
01520                 motor[TIRE_BL].pwm = normal;
01521                 motor[TIRE_BL].dir = BACK;
01522                 if(count>50000) {
01523                     cross++;
01524                     count=0;
01525                 }
01526                 mtc2=true;
01527                 if(cross==2) {
01528                     mode=22;
01529                     cross=0;
01530                     count=0;
01531                     fast=40;
01532                     normal=30;
01533                     slow=20;
01534                 }
01535                 break;
01536             case 0:
01537                 motor[TIRE_FR].pwm = fast;
01538                 motor[TIRE_FR].dir = FOR;
01539                 motor[TIRE_FL].pwm = fast;
01540                 motor[TIRE_FL].dir = FOR;
01541                 motor[TIRE_BR].pwm = fast;
01542                 motor[TIRE_BR].dir = BACK;
01543                 motor[TIRE_BL].pwm = fast;
01544                 motor[TIRE_BL].dir = BACK;
01545                 mtc2=true;
01546                 break;
01547             case 255:
01548                 motor[TIRE_FR].pwm = normal;
01549                 motor[TIRE_FR].dir = FOR;
01550                 motor[TIRE_FL].pwm = fast;
01551                 motor[TIRE_FL].dir = FOR;
01552                 motor[TIRE_BR].pwm = fast;
01553                 motor[TIRE_BR].dir = BACK;
01554                 motor[TIRE_BL].pwm = normal;
01555                 motor[TIRE_BL].dir = BACK;
01556                 mtc2=true;
01557                 break;
01558             case 253:
01559                 motor[TIRE_FR].pwm = 0;
01560                 motor[TIRE_FR].dir = FREE;
01561                 motor[TIRE_FL].pwm = slow;
01562                 motor[TIRE_FL].dir = FOR;
01563                 motor[TIRE_BR].pwm = slow;
01564                 motor[TIRE_BR].dir = BACK;
01565                 motor[TIRE_BL].pwm = 0;
01566                 motor[TIRE_BL].dir = FREE;
01567                 mtc2=true;
01568                 break;
01569             case 254:
01570                 motor[TIRE_FR].pwm = 0;
01571                 motor[TIRE_FR].dir = FREE;
01572                 motor[TIRE_FL].pwm = normal;
01573                 motor[TIRE_FL].dir = FOR;
01574                 motor[TIRE_BR].pwm = normal;
01575                 motor[TIRE_BR].dir = BACK;
01576                 motor[TIRE_BL].pwm = 0;
01577                 motor[TIRE_BL].dir = FREE;
01578                 mtc2=true;
01579                 break;
01580             case 1:
01581                 motor[TIRE_FR].pwm = fast;
01582                 motor[TIRE_FR].dir = FOR;
01583                 motor[TIRE_FL].pwm = slow;
01584                 motor[TIRE_FL].dir = FOR;
01585                 motor[TIRE_BR].pwm = slow;
01586                 motor[TIRE_BR].dir = BACK;
01587                 motor[TIRE_BL].pwm = fast;
01588                 motor[TIRE_BL].dir = BACK;
01589                 mtc2=true;
01590                 break;
01591             case 3:
01592                 motor[TIRE_FR].pwm = slow;
01593                 motor[TIRE_FR].dir = FOR;
01594                 motor[TIRE_FL].pwm = 0;
01595                 motor[TIRE_FL].dir = FREE;
01596                 motor[TIRE_BR].pwm = 0;
01597                 motor[TIRE_BR].dir = FREE;
01598                 motor[TIRE_BL].pwm = slow;
01599                 motor[TIRE_BL].dir = BACK;
01600                 mtc2=true;
01601                 break;
01602             case 2:
01603                 motor[TIRE_FR].pwm = normal;
01604                 motor[TIRE_FR].dir = FOR;
01605                 motor[TIRE_FL].pwm = 0;
01606                 motor[TIRE_FL].dir = FREE;
01607                 motor[TIRE_BR].pwm = 0;
01608                 motor[TIRE_BR].dir = FREE;
01609                 motor[TIRE_BL].pwm = normal;
01610                 motor[TIRE_BL].dir = BACK;
01611                 mtc2=true;
01612                 break;
01613         }
01614         if(g[2]!=98&&mtc2==true) {
01615             switch(g[3]) {
01616                 case 0:
01617                     mtc2=false;
01618                     break;
01619                 case 255:
01620                     motor[TIRE_BL].pwm += 5;
01621                     mtc2=false;
01622                     break;
01623                 case 253:
01624                     motor[TIRE_BL].pwm += 10;
01625                     mtc2=false;
01626                     break;
01627                 case 254:
01628                     motor[TIRE_BL].pwm += 20;
01629                     motor[TIRE_FL].pwm = 0;
01630                     mtc2=false;
01631                     break;
01632                 case 1:
01633                     motor[TIRE_FL].pwm += 5;
01634                     mtc2=false;
01635                     break;
01636                 case 3:
01637                     motor[TIRE_FL].pwm += 10;
01638                     mtc2=false;
01639                     break;
01640                 case 2:
01641                     motor[TIRE_FL].pwm += 20;
01642                     motor[TIRE_BL].pwm = 0;
01643                     mtc2=false;
01644                     break;
01645             }
01646         }
01647 
01648     } else if(mode==21) {
01649         motor[TIRE_FR].pwm = 30;
01650         motor[TIRE_FR].dir = BRAKE;
01651         motor[TIRE_FL].pwm = 30;
01652         motor[TIRE_FL].dir = BRAKE;
01653         motor[TIRE_BR].pwm = 30;
01654         motor[TIRE_BR].dir = BRAKE;
01655         motor[TIRE_BL].pwm = 30;
01656         motor[TIRE_BL].dir = BRAKE;
01657         if(count<=100000) {
01658             mode=22;
01659         }
01660     }
01661     kiri=true;
01662     if(kiri) {
01663         if(mode==22) {
01664             motor[TIRE_FR].pwm = 100;
01665             motor[TIRE_FR].dir = BRAKE;
01666             motor[TIRE_FL].pwm = 100;
01667             motor[TIRE_FL].dir = BRAKE;
01668             motor[TIRE_BR].pwm = 100;
01669             motor[TIRE_BR].dir = BRAKE;
01670             motor[TIRE_BL].pwm = 100;
01671             motor[TIRE_BL].dir = BRAKE;
01672             if(count>=10000) {
01673                 mode=23;
01674             }
01675         } else if(mode==23) {
01676             motor[TIRE_FR].pwm = 20;
01677             motor[TIRE_FR].dir = BACK;
01678             motor[TIRE_FL].pwm = 20;
01679             motor[TIRE_FL].dir = BACK;
01680             motor[TIRE_BR].pwm = 20;
01681             motor[TIRE_BR].dir = FOR;
01682             motor[TIRE_BL].pwm = 20;
01683             motor[TIRE_BL].dir = FOR;
01684             if(g[2]==98) {
01685                 mode=24;
01686                 count=0;
01687             }
01688         } else if(mode==24) {
01689             motor[TIRE_FR].pwm = 20;
01690             motor[TIRE_FR].dir = BACK;
01691             motor[TIRE_FL].pwm = 20;
01692             motor[TIRE_FL].dir = BACK;
01693             motor[TIRE_BR].pwm = 20;
01694             motor[TIRE_BR].dir = FOR;
01695             motor[TIRE_BL].pwm = 20;
01696             motor[TIRE_BL].dir = FOR;
01697             if(g[2]!=98) {
01698                 count=0;
01699                 mode=25;
01700             }
01701         } else if(mode==25) {
01702             motor[TIRE_FR].pwm = 10;
01703             motor[TIRE_FR].dir = BRAKE;
01704             motor[TIRE_FL].pwm = 10;
01705             motor[TIRE_FL].dir = BRAKE;
01706             motor[TIRE_BR].pwm = 10;
01707             motor[TIRE_BR].dir = BRAKE;
01708             motor[TIRE_BL].pwm = 10;
01709             motor[TIRE_BL].dir = BRAKE;
01710             if(count>=10000) {
01711                 mode=26;
01712                 count=0;
01713             }
01714         } else if(mode==26) {
01715             motor[TIRE_FR].pwm = 20;
01716             motor[TIRE_FR].dir = FOR;
01717             motor[TIRE_FL].pwm = 20;
01718             motor[TIRE_FL].dir = FOR;
01719             motor[TIRE_BR].pwm = 20;
01720             motor[TIRE_BR].dir = BACK;
01721             motor[TIRE_BL].pwm = 20;
01722             motor[TIRE_BL].dir = BACK;
01723             if(count>=5000) {
01724                 mode=27;
01725                 count=0;
01726             }
01727         } else if(mode==27) {
01728             motor[TIRE_FR].pwm = 20;
01729             motor[TIRE_FR].dir = FOR;
01730             motor[TIRE_FL].pwm = 20;
01731             motor[TIRE_FL].dir = FOR;
01732             motor[TIRE_BR].pwm = 20;
01733             motor[TIRE_BR].dir = FOR;
01734             motor[TIRE_BL].pwm = 20;
01735             motor[TIRE_BL].dir = FOR;
01736             //どれくらい回転するかって値
01737             if(count>=4000) {
01738                 count=0;
01739                 mode=28;
01740             }
01741         } else if(mode==28) {
01742             motor[TIRE_FR].pwm = 10;
01743             motor[TIRE_FR].dir = BRAKE;
01744             motor[TIRE_FL].pwm = 10;
01745             motor[TIRE_FL].dir = BRAKE;
01746             motor[TIRE_BR].pwm = 10;
01747             motor[TIRE_BR].dir = BRAKE;
01748             motor[TIRE_BL].pwm = 10;
01749             motor[TIRE_BL].dir = BRAKE;
01750             if(count>=10000) {
01751                 mode=30;
01752                 count=0;
01753             }
01754         }
01755     } else {
01756         if(mode==22) {
01757             motor[TIRE_FR].pwm = 0;
01758             motor[TIRE_FR].dir = FREE;
01759             motor[TIRE_FL].pwm = 25;
01760             motor[TIRE_FL].dir = FOR;
01761             motor[TIRE_BR].pwm = 25;
01762             motor[TIRE_BR].dir = BACK;
01763             motor[TIRE_BL].pwm = 0;
01764             motor[TIRE_BL].dir = FREE;
01765             if(g[0]==0&&count>=10000) {
01766                 mode=30;
01767                 count=0;
01768                 cros=0;
01769             }
01770         }
01771     }
01772     if(mode==30) {
01773         motor[TIRE_FR].pwm = 60;
01774         motor[TIRE_FR].dir = FOR;
01775         motor[TIRE_FL].pwm = 60;
01776         motor[TIRE_FL].dir = BACK;
01777         motor[TIRE_BR].pwm = 60;
01778         motor[TIRE_BR].dir = FOR;
01779         motor[TIRE_BL].pwm = 57;
01780         motor[TIRE_BL].dir = BACK;
01781         if(g[1]==98) {
01782             mode=31;
01783             count=0;
01784         }
01785     } else if(mode==31) {
01786         motor[TIRE_FR].pwm = 0;
01787         motor[TIRE_FR].dir = FREE;
01788         motor[TIRE_FL].pwm = 0;
01789         motor[TIRE_FL].dir = FREE;
01790         motor[TIRE_BR].pwm = 0;
01791         motor[TIRE_BR].dir = FREE;
01792         motor[TIRE_BL].pwm = 0;
01793         motor[TIRE_BL].dir = FREE;
01794         if(count>=5000) {
01795             mode=32;
01796             count=0;
01797         }
01798     } else if(mode==32) {
01799         motor[TIRE_FR].pwm = 10;
01800         motor[TIRE_FR].dir = BRAKE;
01801         motor[TIRE_FL].pwm = 10;
01802         motor[TIRE_FL].dir = BRAKE;
01803         motor[TIRE_BR].pwm = 10;
01804         motor[TIRE_BR].dir = BRAKE;
01805         motor[TIRE_BL].pwm = 10;
01806         motor[TIRE_BL].dir = BRAKE;
01807         if(count>=30000) {
01808             mode=33;
01809             count=0;
01810         }
01811     } else if(mode==33) {
01812         motor[TIRE_FR].pwm = 30;
01813         motor[TIRE_FR].dir = FOR;
01814         motor[TIRE_FL].pwm = 0;
01815         motor[TIRE_FL].dir = FREE;
01816         motor[TIRE_BR].pwm = 0;
01817         motor[TIRE_BR].dir = FREE;
01818         motor[TIRE_BL].pwm = 30;
01819         motor[TIRE_BL].dir = BACK;
01820         if(g[3]==0&&count>=20000) {
01821             count=0;
01822             if(Twsh==2) {
01823                 solenoid.solenoid2 = SOLENOID_OFF;
01824             }
01825             mode=40;
01826         }
01827     } else if(mode==40) {
01828         if(LIB) {
01829             mode=42;
01830         }
01831         switch(g[3]) {
01832             case 98:
01833                 motor[TIRE_FR].pwm = normal;
01834                 motor[TIRE_FR].dir = BACK;
01835                 motor[TIRE_FL].pwm = normal;
01836                 motor[TIRE_FL].dir = BACK;
01837                 motor[TIRE_BR].pwm = normal;
01838                 motor[TIRE_BR].dir = FOR;
01839                 motor[TIRE_BL].pwm = normal;
01840                 motor[TIRE_BL].dir = FOR;
01841                 if(count>=10000) {
01842                     cross++;
01843                     count=0;
01844                 }
01845                 mtc2=true;
01846                 if(cross==2) {
01847                     mode=41;
01848                     count=0;
01849                     cross=0;
01850                 }
01851                 break;
01852             case 0:
01853                 motor[TIRE_FR].pwm = fast;
01854                 motor[TIRE_FR].dir = BACK;
01855                 motor[TIRE_FL].pwm = fast;
01856                 motor[TIRE_FL].dir = BACK;
01857                 motor[TIRE_BR].pwm = fast;
01858                 motor[TIRE_BR].dir = FOR;
01859                 motor[TIRE_BL].pwm = fast;
01860                 motor[TIRE_BL].dir = FOR;
01861                 mtc2=true;
01862                 break;
01863             case 255:
01864                 motor[TIRE_FR].pwm = slow;
01865                 motor[TIRE_FR].dir = BACK;
01866                 motor[TIRE_FL].pwm = fast;
01867                 motor[TIRE_FL].dir = BACK;
01868                 motor[TIRE_BR].pwm = fast;
01869                 motor[TIRE_BR].dir = FOR;
01870                 motor[TIRE_BL].pwm = slow;
01871                 motor[TIRE_BL].dir = FOR;
01872                 mtc2=true;
01873                 break;
01874             case 253:
01875                 motor[TIRE_FR].pwm = 0;
01876                 motor[TIRE_FR].dir = FREE;
01877                 motor[TIRE_FL].pwm = slow;
01878                 motor[TIRE_FL].dir = BACK;
01879                 motor[TIRE_BR].pwm = slow;
01880                 motor[TIRE_BR].dir = FOR;
01881                 motor[TIRE_BL].pwm = 0;
01882                 motor[TIRE_BL].dir = FREE;
01883                 mtc2=true;
01884                 break;
01885             case 254:
01886                 motor[TIRE_FR].pwm = 0;
01887                 motor[TIRE_FR].dir = FREE;
01888                 motor[TIRE_FL].pwm = normal;
01889                 motor[TIRE_FL].dir = BACK;
01890                 motor[TIRE_BR].pwm = normal;
01891                 motor[TIRE_BR].dir = FOR;
01892                 motor[TIRE_BL].pwm = 0;
01893                 motor[TIRE_BL].dir = FREE;
01894                 mtc2=true;
01895                 break;
01896             case 1:
01897                 motor[TIRE_FR].pwm = fast;
01898                 motor[TIRE_FR].dir = BACK;
01899                 motor[TIRE_FL].pwm = slow;
01900                 motor[TIRE_FL].dir = BACK;
01901                 motor[TIRE_BR].pwm = slow;
01902                 motor[TIRE_BR].dir = FOR;
01903                 motor[TIRE_BL].pwm = fast;
01904                 motor[TIRE_BL].dir = FOR;
01905                 mtc2=true;
01906                 break;
01907             case 3:
01908                 motor[TIRE_FR].pwm = slow;
01909                 motor[TIRE_FR].dir = BACK;
01910                 motor[TIRE_FL].pwm = 0;
01911                 motor[TIRE_FL].dir = FREE;
01912                 motor[TIRE_BR].pwm = 0;
01913                 motor[TIRE_BR].dir = FREE;
01914                 motor[TIRE_BL].pwm = slow;
01915                 motor[TIRE_BL].dir = FOR;
01916                 mtc2=true;
01917                 break;
01918             case 2:
01919                 motor[TIRE_FR].pwm = normal;
01920                 motor[TIRE_FR].dir = BACK;
01921                 motor[TIRE_FL].pwm = 0;
01922                 motor[TIRE_FL].dir = FREE;
01923                 motor[TIRE_BR].pwm = 0;
01924                 motor[TIRE_BR].dir = FREE;
01925                 motor[TIRE_BL].pwm = normal;
01926                 motor[TIRE_BL].dir = FOR;
01927                 mtc2=true;
01928                 break;
01929         }
01930         if(g[2]!=98&&mtc2==true) {
01931             switch(g[2]) {
01932                 case 0:
01933                     mtc2=false;
01934                     break;
01935                 case 255:
01936                     motor[TIRE_FR].pwm += 5;
01937                     mtc2=false;
01938                     break;
01939                 case 253:
01940                     motor[TIRE_FR].pwm += 10;
01941                     mtc2=false;
01942                     break;
01943                 case 254:
01944                     motor[TIRE_FR].pwm += 20;
01945                     motor[TIRE_BR].pwm = 0;
01946                     mtc2=false;
01947                     break;
01948                 case 1:
01949                     motor[TIRE_BR].pwm += 5;
01950                     mtc2=false;
01951                     break;
01952                 case 3:
01953                     motor[TIRE_BR].pwm += 10;
01954                     mtc2=false;
01955                     break;
01956                 case 2:
01957                     motor[TIRE_BR].pwm += 20;
01958                     motor[TIRE_FR].pwm = 0;
01959                     mtc2=false;
01960                     break;
01961             }
01962         }
01963     } else if(mode==41) {
01964         if(LIF||LIB) {
01965             mode=42;
01966         }
01967         motor[TIRE_FR].pwm = 30;
01968         motor[TIRE_FR].dir = BACK;
01969         motor[TIRE_FL].pwm = 0;
01970         motor[TIRE_FL].dir = FREE;
01971         motor[TIRE_BR].pwm = 0;
01972         motor[TIRE_BR].dir = FREE;
01973         motor[TIRE_BL].pwm = 30;
01974         motor[TIRE_BL].dir = FOR;
01975         if(g[1]==0) {
01976             count=0;
01977             mode=70;
01978             normal=60;
01979             fast=70;
01980             slow=40;
01981         }
01982     } else if(mode==42) {
01983         motor[TIRE_FR].pwm = 0;
01984         motor[TIRE_FR].dir = FREE;
01985         motor[TIRE_FL].pwm = 20;
01986         motor[TIRE_FL].dir = FOR;
01987         motor[TIRE_BR].pwm = 20;
01988         motor[TIRE_BR].dir = BACK;
01989         motor[TIRE_BL].pwm = 0;
01990         motor[TIRE_BL].dir = FREE;
01991         if(g[0]==0) {
01992             count=0;
01993             slow=40;
01994         }
01995 
01996     } else if(mode==70)
01997         switch(g[0]) {
01998             case 99:
01999                 if(count>=100000) {
02000                     cross++;
02001                     mode=100;
02002                     count=0;
02003                 }
02004                 if(cross==2) {
02005                     cross=0;
02006                     mode=100;
02007                 }
02008                 break;
02009             case 98:
02010                 motor[TIRE_FR].pwm = normal;
02011                 motor[TIRE_FR].dir = BACK;
02012                 motor[TIRE_FL].pwm = normal;
02013                 motor[TIRE_FL].dir = FOR;
02014                 motor[TIRE_BR].pwm = normal;
02015                 motor[TIRE_BR].dir = BACK;
02016                 motor[TIRE_BL].pwm = normal;
02017                 motor[TIRE_BL].dir = FOR;
02018                 break;
02019             case 0:
02020                 motor[TIRE_FR].pwm = fast;
02021                 motor[TIRE_FR].dir = BACK;
02022                 motor[TIRE_FL].pwm = fast;
02023                 motor[TIRE_FL].dir = FOR;
02024                 motor[TIRE_BR].pwm = fast;
02025                 motor[TIRE_BR].dir = BACK;
02026                 motor[TIRE_BL].pwm = fast;
02027                 motor[TIRE_BL].dir = FOR;
02028                 mtc=true;
02029                 break;
02030             case 255:
02031                 motor[TIRE_FR].pwm = fast;
02032                 motor[TIRE_FR].dir = BACK;
02033                 motor[TIRE_FL].pwm = slow;
02034                 motor[TIRE_FL].dir = FOR;
02035                 motor[TIRE_BR].pwm = slow;
02036                 motor[TIRE_BR].dir = BACK;
02037                 motor[TIRE_BL].pwm = fast;
02038                 motor[TIRE_BL].dir = FOR;
02039                 mtc=true;
02040                 break;
02041             case 253:
02042                 motor[TIRE_FR].pwm = slow;
02043                 motor[TIRE_FR].dir = BACK;
02044                 motor[TIRE_FL].pwm = 0;
02045                 motor[TIRE_FL].dir = FREE;
02046                 motor[TIRE_BR].pwm = 0;
02047                 motor[TIRE_BR].dir = FREE;
02048                 motor[TIRE_BL].pwm = slow;
02049                 motor[TIRE_BL].dir = FOR;
02050                 mtc=true;
02051                 break;
02052             case 254:
02053                 motor[TIRE_FR].pwm = normal;
02054                 motor[TIRE_FR].dir = BACK;
02055                 motor[TIRE_FL].pwm = 0;
02056                 motor[TIRE_FL].dir = FREE;
02057                 motor[TIRE_BR].pwm = 0;
02058                 motor[TIRE_BR].dir = FREE;
02059                 motor[TIRE_BL].pwm = normal;
02060                 motor[TIRE_BL].dir = FOR;
02061                 mtc=true;
02062                 break;
02063             case 1:
02064                 motor[TIRE_FR].pwm = slow;
02065                 motor[TIRE_FR].dir = BACK;
02066                 motor[TIRE_FL].pwm = fast;
02067                 motor[TIRE_FL].dir = FOR;
02068                 motor[TIRE_BR].pwm = fast;
02069                 motor[TIRE_BR].dir = BACK;
02070                 motor[TIRE_BL].pwm = slow;
02071                 motor[TIRE_BL].dir = FOR;
02072                 mtc=true;
02073                 break;
02074             case 3:
02075                 motor[TIRE_FR].pwm = 0;
02076                 motor[TIRE_FR].dir = FREE;
02077                 motor[TIRE_FL].pwm = slow;
02078                 motor[TIRE_FL].dir = FOR;
02079                 motor[TIRE_BR].pwm = slow;
02080                 motor[TIRE_BR].dir = BACK;
02081                 motor[TIRE_BL].pwm = 0;
02082                 motor[TIRE_BL].dir = FREE;
02083                 mtc=true;
02084                 break;
02085             case 2:
02086                 motor[TIRE_FR].pwm = 0;
02087                 motor[TIRE_FR].dir = FREE;
02088                 motor[TIRE_FL].pwm = normal;
02089                 motor[TIRE_FL].dir = FOR;
02090                 motor[TIRE_BR].pwm = normal;
02091                 motor[TIRE_BR].dir = BACK;
02092                 motor[TIRE_BL].pwm = 0;
02093                 motor[TIRE_BL].dir = FREE;
02094                 mtc=true;
02095                 break;
02096         }
02097     if(g[1]!=98&&mtc==true) {
02098         switch(g[1]) {
02099             case 0:
02100                 mtc=false;
02101                 break;
02102             case 255:
02103                 motor[TIRE_BR].pwm += 5;
02104                 mtc=false;
02105                 break;
02106             case 253:
02107                 motor[TIRE_BR].pwm += 10;
02108                 mtc=false;
02109                 break;
02110             case 254:
02111                 motor[TIRE_BR].pwm += 20;
02112                 motor[TIRE_BL].pwm = 0;
02113                 mtc=false;
02114                 break;
02115             case 1:
02116                 motor[TIRE_BL].pwm += 5;
02117                 mtc=false;
02118                 break;
02119             case 3:
02120                 motor[TIRE_BL].pwm += 10;
02121                 mtc=false;
02122                 break;
02123             case 2:
02124                 motor[TIRE_BL].pwm += 20;
02125                 motor[TIRE_BR].pwm = 0;
02126                 mtc=false;
02127                 break;
02128         }
02129     } else if(mode==100) {
02130         motor[TIRE_FR].pwm = 20;
02131         motor[TIRE_FR].dir = BACK;
02132         motor[TIRE_FL].pwm = 20;
02133         motor[TIRE_FL].dir = FOR;
02134         motor[TIRE_BR].pwm = 23;
02135         motor[TIRE_BR].dir = BACK;
02136         motor[TIRE_BL].pwm = 20;
02137         motor[TIRE_BL].dir = FOR;
02138     }
02139 }
02140 #endif
02141 
02142 #if USE_PROCESS_NUM>5
02143 static void Process5()
02144 {
02145 }
02146 #endif
02147 
02148 #if USE_PROCESS_NUM>6
02149 static void Process6()
02150 {
02151 //   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]);
02152     LedMode(3);
02153     count++;
02154     if(mode==1) {//スタートゾーンから白線検知
02155         fast=40;
02156         normal=30;
02157         slow=20;
02158         motor[TIRE_FR].pwm = normal;
02159         motor[TIRE_FR].dir = BACK;
02160         motor[TIRE_FL].pwm = normal;
02161         motor[TIRE_FL].dir = FOR;
02162         motor[TIRE_BR].pwm = normal;
02163         motor[TIRE_BR].dir = BACK;
02164         motor[TIRE_BL].pwm = normal;
02165         motor[TIRE_BL].dir = FOR;
02166         if(g[2]==0&&count>=100000) {
02167             count=100000;
02168             cross=0;
02169             mode=3;
02170         }
02171     } else if(mode==3) { //横ライントレースから縦ライントレースへ
02172         motor[TIRE_FR].pwm = 0;
02173         motor[TIRE_FR].dir = FREE;
02174         motor[TIRE_FL].pwm = 25;
02175         motor[TIRE_FL].dir = FOR;
02176         motor[TIRE_BR].pwm = 25;
02177         motor[TIRE_BR].dir = BACK;
02178         motor[TIRE_BL].pwm = 0;
02179         motor[TIRE_BL].dir = FREE;
02180         if(g[0]==0) {
02181             mode=10;
02182             count=100000;
02183         }
02184     } else if(mode==10) { //縦ライントレース
02185         if(counts==false&&g[2]==0) {
02186             cros++;
02187             counts=true;
02188         }
02189         if(counts==true&&g[2]==99) {
02190             counts=false;
02191         }
02192 
02193         if(cros==Twsh) {
02194             mode=11;
02195             count=0;
02196             cros=0;
02197         }//ゆっくりモードに入れなかった時の保険
02198         switch(g[0]) {
02199             case 98:
02200                 motor[TIRE_FR].pwm = normal;
02201                 motor[TIRE_FR].dir = BACK;
02202                 motor[TIRE_FL].pwm = normal;
02203                 motor[TIRE_FL].dir = FOR;
02204                 motor[TIRE_BR].pwm = normal;
02205                 motor[TIRE_BR].dir = BACK;
02206                 motor[TIRE_BL].pwm = normal;
02207                 motor[TIRE_BL].dir = FOR;
02208                 mtc=true;
02209                 if(count>80000) {
02210                     cross++;
02211                     count=0;
02212                 }
02213                 if(cross==Twsh) {
02214                     mode=11;
02215                     count=0;
02216                     cross=0;
02217                 }
02218                 break;
02219             case 0:
02220                 motor[TIRE_FR].pwm = fast;
02221                 motor[TIRE_FR].dir = BACK;
02222                 motor[TIRE_FL].pwm = fast;
02223                 motor[TIRE_FL].dir = FOR;
02224                 motor[TIRE_BR].pwm = fast;
02225                 motor[TIRE_BR].dir = BACK;
02226                 motor[TIRE_BL].pwm = fast;
02227                 motor[TIRE_BL].dir = FOR;
02228                 mtc=true;
02229                 break;
02230             case 255:
02231                 motor[TIRE_FR].pwm = fast;
02232                 motor[TIRE_FR].dir = BACK;
02233                 motor[TIRE_FL].pwm = slow;
02234                 motor[TIRE_FL].dir = FOR;
02235                 motor[TIRE_BR].pwm = slow;
02236                 motor[TIRE_BR].dir = BACK;
02237                 motor[TIRE_BL].pwm = fast;
02238                 motor[TIRE_BL].dir = FOR;
02239                 mtc=true;
02240                 break;
02241             case 253:
02242                 motor[TIRE_FR].pwm = slow;
02243                 motor[TIRE_FR].dir = BACK;
02244                 motor[TIRE_FL].pwm = 0;
02245                 motor[TIRE_FL].dir = FREE;
02246                 motor[TIRE_BR].pwm = 0;
02247                 motor[TIRE_BR].dir = FREE;
02248                 motor[TIRE_BL].pwm = slow;
02249                 motor[TIRE_BL].dir = FOR;
02250                 mtc=true;
02251                 break;
02252             case 254:
02253                 motor[TIRE_FR].pwm = normal;
02254                 motor[TIRE_FR].dir = BACK;
02255                 motor[TIRE_FL].pwm = 0;
02256                 motor[TIRE_FL].dir = FREE;
02257                 motor[TIRE_BR].pwm = 0;
02258                 motor[TIRE_BR].dir = FREE;
02259                 motor[TIRE_BL].pwm = normal;
02260                 motor[TIRE_BL].dir = FOR;
02261                 mtc=true;
02262                 break;
02263             case 1:
02264                 motor[TIRE_FR].pwm = slow;
02265                 motor[TIRE_FR].dir = BACK;
02266                 motor[TIRE_FL].pwm = fast;
02267                 motor[TIRE_FL].dir = FOR;
02268                 motor[TIRE_BR].pwm = fast;
02269                 motor[TIRE_BR].dir = BACK;
02270                 motor[TIRE_BL].pwm = slow;
02271                 motor[TIRE_BL].dir = FOR;
02272                 mtc=true;
02273                 break;
02274             case 3:
02275                 motor[TIRE_FR].pwm = 0;
02276                 motor[TIRE_FR].dir = FREE;
02277                 motor[TIRE_FL].pwm = slow;
02278                 motor[TIRE_FL].dir = FOR;
02279                 motor[TIRE_BR].pwm = slow;
02280                 motor[TIRE_BR].dir = BACK;
02281                 motor[TIRE_BL].pwm = 0;
02282                 motor[TIRE_BL].dir = FREE;
02283                 mtc=true;
02284                 break;
02285             case 2:
02286                 motor[TIRE_FR].pwm = 0;
02287                 motor[TIRE_FR].dir = FREE;
02288                 motor[TIRE_FL].pwm = normal;
02289                 motor[TIRE_FL].dir = FOR;
02290                 motor[TIRE_BR].pwm = normal;
02291                 motor[TIRE_BR].dir = BACK;
02292                 motor[TIRE_BL].pwm = 0;
02293                 motor[TIRE_BL].dir = FREE;
02294                 mtc=true;
02295                 break;
02296         }
02297         if(g[0]!=98&&mtc==true) {
02298             switch(g[1]) {
02299                 case 0:
02300                     mtc=false;
02301                     break;
02302                 case 255:
02303                     motor[TIRE_BR].pwm += 5;
02304                     mtc=false;
02305                     break;
02306                 case 253:
02307                     motor[TIRE_BR].pwm += 10;
02308                     mtc=false;
02309                     break;
02310                 case 254:
02311                     motor[TIRE_BR].pwm += 20;
02312                     motor[TIRE_BL].pwm = 0;
02313                     mtc=false;
02314                     break;
02315                 case 1:
02316                     motor[TIRE_BL].pwm += 5;
02317                     mtc=false;
02318                     break;
02319                 case 3:
02320                     motor[TIRE_BL].pwm += 10;
02321                     mtc=false;
02322                     break;
02323                 case 2:
02324                     motor[TIRE_BL].pwm += 20;
02325                     motor[TIRE_BR].pwm = 0;
02326                     mtc=false;
02327                     break;
02328             }
02329         }
02330     } else if(mode==11) {
02331         motor[TIRE_FR].pwm = 0;
02332         motor[TIRE_FR].dir = FREE;
02333         motor[TIRE_FL].pwm = normal;
02334         motor[TIRE_FL].dir = FOR;
02335         motor[TIRE_BR].pwm = normal;
02336         motor[TIRE_BR].dir = BACK;
02337         motor[TIRE_BL].pwm = 0;
02338         motor[TIRE_BL].dir = FREE;
02339         if(g[2]==0) {
02340             count=0;
02341             cross=0;
02342             mode=20;
02343         }
02344     } else if(mode==20) {
02345 
02346         if(Twsh==2) {
02347             solenoid.solenoid2 = SOLENOID_ON;
02348         }
02349         if(counts==false&&g[0]==0) {
02350             cros++;
02351             counts=true;
02352         }
02353         if(counts==true&&g[0]==99) {
02354             counts=false;
02355         }
02356 
02357         if(cros==2) {
02358 
02359             // mode=21;
02360             // count=0;
02361             // cros=0;
02362         }//ゆっくりモードに入れなかった時の保険
02363         if(g[0]==0) {
02364             fast=35;
02365             normal=20;
02366             slow=15;
02367         }
02368         switch(g[2]) {
02369             case 98:
02370                 motor[TIRE_FR].pwm = normal;
02371                 motor[TIRE_FR].dir = FOR;
02372                 motor[TIRE_FL].pwm = normal;
02373                 motor[TIRE_FL].dir = FOR;
02374                 motor[TIRE_BR].pwm = normal;
02375                 motor[TIRE_BR].dir = BACK;
02376                 motor[TIRE_BL].pwm = normal;
02377                 motor[TIRE_BL].dir = BACK;
02378                 if(count>30000) {
02379                     cross++;
02380                     count=0;
02381                 }
02382                 mtc2=true;
02383                 if(cross==2) {
02384                     mode=22;
02385                     count=0;
02386                     cross=0;
02387                     fast=40;
02388                     normal=30;
02389                     slow=20;
02390                 }
02391                 break;
02392             case 0:
02393                 motor[TIRE_FR].pwm = fast;
02394                 motor[TIRE_FR].dir = FOR;
02395                 motor[TIRE_FL].pwm = fast;
02396                 motor[TIRE_FL].dir = FOR;
02397                 motor[TIRE_BR].pwm = fast;
02398                 motor[TIRE_BR].dir = BACK;
02399                 motor[TIRE_BL].pwm = fast;
02400                 motor[TIRE_BL].dir = BACK;
02401                 mtc2=true;
02402                 break;
02403             case 255:
02404                 motor[TIRE_FR].pwm = normal;
02405                 motor[TIRE_FR].dir = FOR;
02406                 motor[TIRE_FL].pwm = fast;
02407                 motor[TIRE_FL].dir = FOR;
02408                 motor[TIRE_BR].pwm = fast;
02409                 motor[TIRE_BR].dir = BACK;
02410                 motor[TIRE_BL].pwm = normal;
02411                 motor[TIRE_BL].dir = BACK;
02412                 mtc2=true;
02413                 break;
02414             case 253:
02415                 motor[TIRE_FR].pwm = 0;
02416                 motor[TIRE_FR].dir = FREE;
02417                 motor[TIRE_FL].pwm = slow;
02418                 motor[TIRE_FL].dir = FOR;
02419                 motor[TIRE_BR].pwm = slow;
02420                 motor[TIRE_BR].dir = BACK;
02421                 motor[TIRE_BL].pwm = 0;
02422                 motor[TIRE_BL].dir = FREE;
02423                 mtc2=true;
02424                 break;
02425             case 254:
02426                 motor[TIRE_FR].pwm = 0;
02427                 motor[TIRE_FR].dir = FREE;
02428                 motor[TIRE_FL].pwm = normal;
02429                 motor[TIRE_FL].dir = FOR;
02430                 motor[TIRE_BR].pwm = normal;
02431                 motor[TIRE_BR].dir = BACK;
02432                 motor[TIRE_BL].pwm = 0;
02433                 motor[TIRE_BL].dir = FREE;
02434                 mtc2=true;
02435                 break;
02436             case 1:
02437                 motor[TIRE_FR].pwm = fast;
02438                 motor[TIRE_FR].dir = FOR;
02439                 motor[TIRE_FL].pwm = slow;
02440                 motor[TIRE_FL].dir = FOR;
02441                 motor[TIRE_BR].pwm = slow;
02442                 motor[TIRE_BR].dir = BACK;
02443                 motor[TIRE_BL].pwm = fast;
02444                 motor[TIRE_BL].dir = BACK;
02445                 mtc2=true;
02446                 break;
02447             case 3:
02448                 motor[TIRE_FR].pwm = slow;
02449                 motor[TIRE_FR].dir = FOR;
02450                 motor[TIRE_FL].pwm = 0;
02451                 motor[TIRE_FL].dir = FREE;
02452                 motor[TIRE_BR].pwm = 0;
02453                 motor[TIRE_BR].dir = FREE;
02454                 motor[TIRE_BL].pwm = slow;
02455                 motor[TIRE_BL].dir = BACK;
02456                 mtc2=true;
02457                 break;
02458             case 2:
02459                 motor[TIRE_FR].pwm = normal;
02460                 motor[TIRE_FR].dir = FOR;
02461                 motor[TIRE_FL].pwm = 0;
02462                 motor[TIRE_FL].dir = FREE;
02463                 motor[TIRE_BR].pwm = 0;
02464                 motor[TIRE_BR].dir = FREE;
02465                 motor[TIRE_BL].pwm = normal;
02466                 motor[TIRE_BL].dir = BACK;
02467                 mtc2=true;
02468                 break;
02469         }
02470         if(g[2]!=98&&mtc2==true) {
02471             switch(g[3]) {
02472                 case 0:
02473                     mtc2=false;
02474                     break;
02475                 case 255:
02476                     motor[TIRE_BL].pwm += 5;
02477                     mtc2=false;
02478                     break;
02479                 case 253:
02480                     motor[TIRE_BL].pwm += 10;
02481                     mtc2=false;
02482                     break;
02483                 case 254:
02484                     motor[TIRE_BL].pwm += 20;
02485                     motor[TIRE_FL].pwm = 0;
02486                     mtc2=false;
02487                     break;
02488                 case 1:
02489                     motor[TIRE_FL].pwm += 5;
02490                     mtc2=false;
02491                     break;
02492                 case 3:
02493                     motor[TIRE_FL].pwm += 10;
02494                     mtc2=false;
02495                     break;
02496                 case 2:
02497                     motor[TIRE_FL].pwm += 20;
02498                     motor[TIRE_BL].pwm = 0;
02499                     mtc2=false;
02500                     break;
02501             }
02502         }
02503     } else if(mode==21) {
02504         motor[TIRE_FR].pwm = 30;
02505         motor[TIRE_FR].dir = FOR;
02506         motor[TIRE_FL].pwm = 30;
02507         motor[TIRE_FL].dir = FOR;
02508         motor[TIRE_BR].pwm = 30;
02509         motor[TIRE_BR].dir = BACK;
02510         motor[TIRE_BL].pwm = 30;
02511         motor[TIRE_BL].dir = BACK;
02512         if(g[2]==98&&count>=10000) {
02513             mode=22;
02514             count=0;
02515         }
02516     }
02517     kiri=true;
02518     if(kiri) {
02519         if(mode==22) {
02520             motor[TIRE_FR].pwm = 100;
02521             motor[TIRE_FR].dir = BRAKE;
02522             motor[TIRE_FL].pwm = 100;
02523             motor[TIRE_FL].dir = BRAKE;
02524             motor[TIRE_BR].pwm = 100;
02525             motor[TIRE_BR].dir = BRAKE;
02526             motor[TIRE_BL].pwm = 100;
02527             motor[TIRE_BL].dir = BRAKE;
02528             if(count>=10000) {
02529                 mode=23;
02530             }
02531         } else if(mode==23) {
02532             motor[TIRE_FR].pwm = 20;
02533             motor[TIRE_FR].dir = BACK;
02534             motor[TIRE_FL].pwm = 20;
02535             motor[TIRE_FL].dir = BACK;
02536             motor[TIRE_BR].pwm = 20;
02537             motor[TIRE_BR].dir = FOR;
02538             motor[TIRE_BL].pwm = 20;
02539             motor[TIRE_BL].dir = FOR;
02540             if(g[2]==98) {
02541                 mode=24;
02542                 count=0;
02543             }
02544         } else if(mode==24) {
02545             motor[TIRE_FR].pwm = 20;
02546             motor[TIRE_FR].dir = BACK;
02547             motor[TIRE_FL].pwm = 20;
02548             motor[TIRE_FL].dir = BACK;
02549             motor[TIRE_BR].pwm = 20;
02550             motor[TIRE_BR].dir = FOR;
02551             motor[TIRE_BL].pwm = 20;
02552             motor[TIRE_BL].dir = FOR;
02553             if(g[2]!=98) {
02554                 count=0;
02555                 mode=25;
02556             }
02557         } else if(mode==25) {
02558             motor[TIRE_FR].pwm = 10;
02559             motor[TIRE_FR].dir = BRAKE;
02560             motor[TIRE_FL].pwm = 10;
02561             motor[TIRE_FL].dir = BRAKE;
02562             motor[TIRE_BR].pwm = 10;
02563             motor[TIRE_BR].dir = BRAKE;
02564             motor[TIRE_BL].pwm = 10;
02565             motor[TIRE_BL].dir = BRAKE;
02566             if(count>=10000) {
02567                 mode=26;
02568                 count=0;
02569             }
02570         } else if(mode==26) {
02571             motor[TIRE_FR].pwm = 20;
02572             motor[TIRE_FR].dir = FOR;
02573             motor[TIRE_FL].pwm = 20;
02574             motor[TIRE_FL].dir = FOR;
02575             motor[TIRE_BR].pwm = 20;
02576             motor[TIRE_BR].dir = BACK;
02577             motor[TIRE_BL].pwm = 20;
02578             motor[TIRE_BL].dir = BACK;
02579             if(count>=5000) {
02580                 mode=27;
02581                 count=0;
02582             }
02583         } else if(mode==27) {
02584             motor[TIRE_FR].pwm = 20;
02585             motor[TIRE_FR].dir = FOR;
02586             motor[TIRE_FL].pwm = 20;
02587             motor[TIRE_FL].dir = FOR;
02588             motor[TIRE_BR].pwm = 20;
02589             motor[TIRE_BR].dir = FOR;
02590             motor[TIRE_BL].pwm = 20;
02591             motor[TIRE_BL].dir = FOR;
02592             //どれくらい回転するかって値
02593             if(count>=100) {
02594                 count=0;
02595                 mode=30;
02596             }
02597         } else if(mode==24) {
02598             motor[TIRE_FR].pwm = 10;
02599             motor[TIRE_FR].dir = BRAKE;
02600             motor[TIRE_FL].pwm = 10;
02601             motor[TIRE_FL].dir = BRAKE;
02602             motor[TIRE_BR].pwm = 10;
02603             motor[TIRE_BR].dir = BRAKE;
02604             motor[TIRE_BL].pwm = 10;
02605             motor[TIRE_BL].dir = BRAKE;
02606             if(count>=10000) {
02607                 mode=30;
02608                 count=0;
02609             }
02610         }
02611     } else {
02612         if(mode==22) {
02613             motor[TIRE_FR].pwm = 0;
02614             motor[TIRE_FR].dir = FREE;
02615             motor[TIRE_FL].pwm = 25;
02616             motor[TIRE_FL].dir = FOR;
02617             motor[TIRE_BR].pwm = 25;
02618             motor[TIRE_BR].dir = BACK;
02619             motor[TIRE_BL].pwm = 0;
02620             motor[TIRE_BL].dir = FREE;
02621             if(g[0]==0&&count>=10000) {
02622                 mode=30;
02623                 count=0;
02624                 cros=0;
02625             }
02626         }
02627     }
02628     if(mode==30) {
02629         motor[TIRE_FR].pwm = 59;
02630         motor[TIRE_FR].dir = BACK;
02631         motor[TIRE_FL].pwm = 62;
02632         motor[TIRE_FL].dir = FOR;
02633         motor[TIRE_BR].pwm = 59;
02634         motor[TIRE_BR].dir = BACK;
02635         motor[TIRE_BL].pwm = 62;
02636         motor[TIRE_BL].dir = FOR;
02637         if(g[0]==98) {
02638             mode=31;
02639             count=0;
02640         }
02641     } else if(mode==31) {
02642         motor[TIRE_FR].pwm = 0;
02643         motor[TIRE_FR].dir = FREE;
02644         motor[TIRE_FL].pwm = 0;
02645         motor[TIRE_FL].dir = FREE;
02646         motor[TIRE_BR].pwm = 0;
02647         motor[TIRE_BR].dir = FREE;
02648         motor[TIRE_BL].pwm = 0;
02649         motor[TIRE_BL].dir = FREE;
02650         if(count>=5000) {
02651             count=0;
02652             mode=32;
02653         }
02654     } else if(mode==32) {
02655         motor[TIRE_FR].pwm = 10;
02656         motor[TIRE_FR].dir = BRAKE;
02657         motor[TIRE_FL].pwm = 10;
02658         motor[TIRE_FL].dir = BRAKE;
02659         motor[TIRE_BR].pwm = 10;
02660         motor[TIRE_BR].dir = BRAKE;
02661         motor[TIRE_BL].pwm = 10;
02662         motor[TIRE_BL].dir = BRAKE;
02663         if(count>=40000) {
02664             mode=33;
02665             count=0;
02666         }
02667     } else if(mode==33) {
02668         motor[TIRE_FR].pwm = 0;
02669         motor[TIRE_FR].dir = BACK;
02670         motor[TIRE_FL].pwm = 30;
02671         motor[TIRE_FL].dir = FOR;
02672         motor[TIRE_BR].pwm = 30;
02673         motor[TIRE_BR].dir = BACK;
02674         motor[TIRE_BL].pwm = 0;
02675         motor[TIRE_BL].dir = FOR;
02676         if(g[3]==0&&count>=20000) {
02677             count=0;
02678             if(Twsh==2) {
02679                 solenoid.solenoid2 = SOLENOID_OFF;
02680             }
02681             mode=40;
02682         }
02683     } else if(mode==40) {
02684         if(LIB) {
02685             mode=42;
02686         }
02687         if(counts==false&&g[0]==0) {
02688             cros++;
02689             counts=true;
02690         }
02691         if(counts==true&&g[0]==99) {
02692             counts=false;
02693         }
02694 
02695         if(cros==1) {
02696             // mode=41;
02697             //count=0;
02698             //cros=0;
02699         }//ゆっくりモードに入れなかった時の保険
02700 
02701         switch(g[3]) {
02702             case 98:
02703                 motor[TIRE_FR].pwm = normal;
02704                 motor[TIRE_FR].dir = BACK;
02705                 motor[TIRE_FL].pwm = normal;
02706                 motor[TIRE_FL].dir = BACK;
02707                 motor[TIRE_BR].pwm = normal;
02708                 motor[TIRE_BR].dir = FOR;
02709                 motor[TIRE_BL].pwm = normal;
02710                 motor[TIRE_BL].dir = FOR;
02711                 if(count>20000) {
02712                     cross++;
02713                     count=0;
02714                 }
02715                 mtc2=true;
02716                 if(cross==2) {
02717                     mode=41;
02718                     count=0;
02719                     cross=0;
02720                 }
02721                 break;
02722             case 0:
02723                 motor[TIRE_FR].pwm = fast;
02724                 motor[TIRE_FR].dir = BACK;
02725                 motor[TIRE_FL].pwm = fast;
02726                 motor[TIRE_FL].dir = BACK;
02727                 motor[TIRE_BR].pwm = fast;
02728                 motor[TIRE_BR].dir = FOR;
02729                 motor[TIRE_BL].pwm = fast;
02730                 motor[TIRE_BL].dir = FOR;
02731                 mtc2=true;
02732                 break;
02733             case 255:
02734                 motor[TIRE_FR].pwm = slow;
02735                 motor[TIRE_FR].dir = BACK;
02736                 motor[TIRE_FL].pwm = fast;
02737                 motor[TIRE_FL].dir = BACK;
02738                 motor[TIRE_BR].pwm = fast;
02739                 motor[TIRE_BR].dir = FOR;
02740                 motor[TIRE_BL].pwm = slow;
02741                 motor[TIRE_BL].dir = FOR;
02742                 mtc2=true;
02743                 break;
02744             case 253:
02745                 motor[TIRE_FR].pwm = 0;
02746                 motor[TIRE_FR].dir = FREE;
02747                 motor[TIRE_FL].pwm = slow;
02748                 motor[TIRE_FL].dir = BACK;
02749                 motor[TIRE_BR].pwm = slow;
02750                 motor[TIRE_BR].dir = FOR;
02751                 motor[TIRE_BL].pwm = 0;
02752                 motor[TIRE_BL].dir = FREE;
02753                 mtc2=true;
02754                 break;
02755             case 254:
02756                 motor[TIRE_FR].pwm = 0;
02757                 motor[TIRE_FR].dir = FREE;
02758                 motor[TIRE_FL].pwm = normal;
02759                 motor[TIRE_FL].dir = BACK;
02760                 motor[TIRE_BR].pwm = normal;
02761                 motor[TIRE_BR].dir = FOR;
02762                 motor[TIRE_BL].pwm = 0;
02763                 motor[TIRE_BL].dir = FREE;
02764                 mtc2=true;
02765                 break;
02766             case 1:
02767                 motor[TIRE_FR].pwm = fast;
02768                 motor[TIRE_FR].dir = BACK;
02769                 motor[TIRE_FL].pwm = slow;
02770                 motor[TIRE_FL].dir = BACK;
02771                 motor[TIRE_BR].pwm = slow;
02772                 motor[TIRE_BR].dir = FOR;
02773                 motor[TIRE_BL].pwm = fast;
02774                 motor[TIRE_BL].dir = FOR;
02775                 mtc2=true;
02776                 break;
02777             case 3:
02778                 motor[TIRE_FR].pwm = slow;
02779                 motor[TIRE_FR].dir = BACK;
02780                 motor[TIRE_FL].pwm = 0;
02781                 motor[TIRE_FL].dir = FREE;
02782                 motor[TIRE_BR].pwm = 0;
02783                 motor[TIRE_BR].dir = FREE;
02784                 motor[TIRE_BL].pwm = slow;
02785                 motor[TIRE_BL].dir = FOR;
02786                 mtc2=true;
02787                 break;
02788             case 2:
02789                 motor[TIRE_FR].pwm = normal;
02790                 motor[TIRE_FR].dir = BACK;
02791                 motor[TIRE_FL].pwm = 0;
02792                 motor[TIRE_FL].dir = FREE;
02793                 motor[TIRE_BR].pwm = 0;
02794                 motor[TIRE_BR].dir = FREE;
02795                 motor[TIRE_BL].pwm = normal;
02796                 motor[TIRE_BL].dir = FOR;
02797                 mtc2=true;
02798                 break;
02799         }
02800         if(g[2]!=98&&mtc2==true) {
02801             switch(g[2]) {
02802                 case 0:
02803                     mtc2=false;
02804                     break;
02805                 case 255:
02806                     motor[TIRE_FL].pwm += 5;
02807                     mtc2=false;
02808                     break;
02809                 case 253:
02810                     motor[TIRE_FL].pwm += 10;
02811                     mtc2=false;
02812                     break;
02813                 case 254:
02814                     motor[TIRE_FL].pwm += 20;
02815                     motor[TIRE_BL].pwm = 0;
02816                     mtc2=false;
02817                     break;
02818                 case 1:
02819                     motor[TIRE_BL].pwm += 5;
02820                     mtc2=false;
02821                     break;
02822                 case 3:
02823                     motor[TIRE_BL].pwm += 10;
02824                     mtc2=false;
02825                     break;
02826                 case 2:
02827                     motor[TIRE_BL].pwm += 20;
02828                     motor[TIRE_FL].pwm = 0;
02829                     mtc2=false;
02830                     break;
02831             }
02832         }
02833     } else if(mode==41) {
02834         if(LIF||LIB) {
02835             mode=42;
02836         }
02837         motor[TIRE_FR].pwm = 0;
02838         motor[TIRE_FR].dir = FREE;
02839         motor[TIRE_FL].pwm = 30;
02840         motor[TIRE_FL].dir = BACK;
02841         motor[TIRE_BR].pwm = 30;
02842         motor[TIRE_BR].dir = FOR;
02843         motor[TIRE_BL].pwm = 0;
02844         motor[TIRE_BL].dir = FREE;
02845         if(g[1]==0) {
02846             count=0;
02847             mode=70;
02848             normal=60;
02849             fast=70;
02850             slow=40;
02851         }
02852     } else if(mode==42) {
02853         motor[TIRE_FR].pwm = 20;
02854         motor[TIRE_FR].dir = FOR;
02855         motor[TIRE_FL].pwm = 0;
02856         motor[TIRE_FL].dir = FREE;
02857         motor[TIRE_BR].pwm = 0;
02858         motor[TIRE_BR].dir = FREE;
02859         motor[TIRE_BL].pwm = 20;
02860         motor[TIRE_BL].dir = BACK;
02861         if(g[1]==0) {
02862             count=0;
02863             mode=70;
02864         }
02865     } else if(mode==70) {
02866         switch(g[1]) {
02867             case 99:
02868                 if(count>=100000) {
02869                     mode=100;
02870                 }
02871                 break;
02872             case 98:
02873                 motor[TIRE_FR].pwm = normal;
02874                 motor[TIRE_FR].dir = FOR;
02875                 motor[TIRE_FL].pwm = normal;
02876                 motor[TIRE_FL].dir = BACK;
02877                 motor[TIRE_BR].pwm = normal;
02878                 motor[TIRE_BR].dir = FOR;
02879                 motor[TIRE_BL].pwm = normal;
02880                 motor[TIRE_BL].dir = BACK;
02881                 break;
02882             case 0:
02883                 motor[TIRE_FR].pwm = 80;
02884                 motor[TIRE_FR].dir = FOR;
02885                 motor[TIRE_FL].pwm = 80;
02886                 motor[TIRE_FL].dir = BACK;
02887                 motor[TIRE_BR].pwm = 80;
02888                 motor[TIRE_BR].dir = FOR;
02889                 motor[TIRE_BL].pwm = 80;
02890                 motor[TIRE_BL].dir = BACK;
02891                 mtc=true;
02892                 break;
02893             case 255:
02894                 motor[TIRE_FR].pwm = fast;
02895                 motor[TIRE_FR].dir = FOR;
02896                 motor[TIRE_FL].pwm = slow;
02897                 motor[TIRE_FL].dir = BACK;
02898                 motor[TIRE_BR].pwm = slow;
02899                 motor[TIRE_BR].dir = FOR;
02900                 motor[TIRE_BL].pwm = fast;
02901                 motor[TIRE_BL].dir = BACK;
02902                 mtc=true;
02903                 break;
02904             case 253:
02905                 motor[TIRE_FR].pwm = slow;
02906                 motor[TIRE_FR].dir = FOR;
02907                 motor[TIRE_FL].pwm = 0;
02908                 motor[TIRE_FL].dir = FREE;
02909                 motor[TIRE_BR].pwm = 0;
02910                 motor[TIRE_BR].dir = FREE;
02911                 motor[TIRE_BL].pwm = slow;
02912                 motor[TIRE_BL].dir = BACK;
02913                 mtc=true;
02914                 break;
02915             case 254:
02916                 motor[TIRE_FR].pwm = normal;
02917                 motor[TIRE_FR].dir = FOR;
02918                 motor[TIRE_FL].pwm = 0;
02919                 motor[TIRE_FL].dir = FREE;
02920                 motor[TIRE_BR].pwm = 0;
02921                 motor[TIRE_BR].dir = FREE;
02922                 motor[TIRE_BL].pwm = normal;
02923                 motor[TIRE_BL].dir = BACK;
02924                 mtc=true;
02925                 break;
02926             case 1:
02927                 motor[TIRE_FR].pwm = slow;
02928                 motor[TIRE_FR].dir = FOR;
02929                 motor[TIRE_FL].pwm = fast;
02930                 motor[TIRE_FL].dir = BACK;
02931                 motor[TIRE_BR].pwm = fast;
02932                 motor[TIRE_BR].dir = FOR;
02933                 motor[TIRE_BL].pwm = slow;
02934                 motor[TIRE_BL].dir = BACK;
02935                 mtc=true;
02936                 break;
02937             case 3:
02938                 motor[TIRE_FR].pwm = 0;
02939                 motor[TIRE_FR].dir = FREE;
02940                 motor[TIRE_FL].pwm = slow;
02941                 motor[TIRE_FL].dir = BACK;
02942                 motor[TIRE_BR].pwm = slow;
02943                 motor[TIRE_BR].dir = FOR;
02944                 motor[TIRE_BL].pwm = 0;
02945                 motor[TIRE_BL].dir = FREE;
02946                 mtc=true;
02947                 break;
02948             case 2:
02949                 motor[TIRE_FR].pwm = 0;
02950                 motor[TIRE_FR].dir = FREE;
02951                 motor[TIRE_FL].pwm = normal;
02952                 motor[TIRE_FL].dir = BACK;
02953                 motor[TIRE_BR].pwm = normal;
02954                 motor[TIRE_BR].dir = FOR;
02955                 motor[TIRE_BL].pwm = 0;
02956                 motor[TIRE_BL].dir = FREE;
02957                 mtc=true;
02958                 break;
02959         }
02960         if(g[1]!=98&&mtc==true) {
02961             switch(g[0]) {
02962                 case 0:
02963                     mtc=false;
02964                     break;
02965                 case 255:
02966                     motor[TIRE_FL].pwm += 5;
02967                     mtc=false;
02968                     break;
02969                 case 253:
02970                     motor[TIRE_FL].pwm += 10;
02971                     mtc=false;
02972                     break;
02973                 case 254:
02974                     motor[TIRE_FL].pwm += 20;
02975                     motor[TIRE_FR].pwm = 0;
02976                     mtc=false;
02977                     break;
02978                 case 1:
02979                     motor[TIRE_FR].pwm += 5;
02980                     mtc=false;
02981                     break;
02982                 case 3:
02983                     motor[TIRE_FR].pwm += 10;
02984                     mtc=false;
02985                     break;
02986                 case 2:
02987                     motor[TIRE_FR].pwm += 20;
02988                     motor[TIRE_FL].pwm = 0;
02989                     mtc=false;
02990                     break;
02991             }
02992         }
02993     } else if(mode==100) {
02994         motor[TIRE_FR].pwm = 20;
02995         motor[TIRE_FR].dir = FOR;
02996         motor[TIRE_FL].pwm = 20;
02997         motor[TIRE_FL].dir = BACK;
02998         motor[TIRE_BR].pwm = 23;
02999         motor[TIRE_BR].dir = FOR;
03000         motor[TIRE_BL].pwm = 20;
03001         motor[TIRE_BL].dir = BACK;
03002     } else {
03003     }
03004 }
03005 #endif
03006 
03007 #if USE_PROCESS_NUM>7
03008 static void Process7()
03009 {
03010 
03011 }
03012 #endif
03013 
03014 #if USE_PROCESS_NUM>8
03015 static void Process8()
03016 {
03017 
03018 }
03019 #endif
03020 
03021 #if USE_PROCESS_NUM>9
03022 static void Process9()
03023 {
03024 
03025 }
03026 #endif
03027 #endif
03028 #pragma endregion PROCESS
03029 
03030 static void AllActuatorReset()
03031 {
03032 
03033 #ifdef USE_SOLENOID
03034     solenoid.all = ALL_SOLENOID_OFF;
03035 #endif
03036 
03037 #ifdef USE_MOTOR
03038     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
03039         motor[i].dir = FREE;
03040         motor[i].pwm = 0;
03041     }
03042 #endif
03043 }
03044 
03045 /*
03046 void BuzzerTimer_func()
03047 {
03048     buzzer = !buzzer;
03049     //LED_DEBUG0 = !LED_DEBUG0;
03050 }
03051 */
03052 void TapeLedEms_func()
03053 {
03054     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
03055 }
03056 
03057 #pragma region USER-DEFINED-FUNCTIONS
03058 
03059 
03060 #pragma endregion