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