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