aa

Dependencies:   mbed

Revision:
33:dd598bb23ad1
Parent:
32:985ec7913f2a
Child:
34:2fce61e3ebc4
diff -r 985ec7913f2a -r dd598bb23ad1 System/Process/Process.cpp
--- a/System/Process/Process.cpp	Tue Oct 22 02:50:54 2019 +0000
+++ b/System/Process/Process.cpp	Mon Oct 28 21:11:17 2019 +0000
@@ -62,11 +62,87 @@
 uint8_t SetStatus(int);
 uint8_t SetPWM(int);
 
+uint8_t Setpwm(int pwmVal)
+{
+    if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
+    else return abs(pwmVal);
+}
+
+int selectnum[16][4]= {
+//1,2,4,8
+    {0,0,0,0},
+    {1,0,0,0},
+    {0,1,0,0},
+    {1,1,0,0},
+    {0,0,1,0},
+    {1,0,1,0},
+    {0,1,1,0},
+    {1,1,1,0},
+    {0,0,0,1},
+    {1,0,0,1},
+    {0,1,0,1},
+    {1,1,0,1},
+    {0,0,1,1},
+    {1,0,1,1},
+    {0,1,1,1},
+    {1,1,1,1}
+};
+
+DigitalIn EMC(PC_13);
+
+void LedMode(int num)
+{   
+/* 
+    POTENTIOMETER::dio[0]= selectnum[num][0];;
+    POTENTIOMETER::dio[1]= selectnum[num][1];;
+    POTENTIOMETER::dio[2]= selectnum[num][2];;
+    POTENTIOMETER::dio[3]= selectnum[num][3];;
+    */
+
+}
+
+bool start_flag = true;
+bool UP_flag = false;
+bool SW_flag = false;
+bool Air_flag = false;
+bool zyouge;
+bool zyougedo;
+bool dz1=true;
+bool dz1i=false;
+bool dz2=true;
+bool dz2i=false;
+bool dz3=true;
+bool dz3i=false;
+bool dz4=true;
+bool dz4i=false;
+bool zone;
+bool through=false;
+bool counts=false;
+bool mtc=false;
+bool mtc2 = false;
+bool emcs=false;
+bool kiri;
+
+int g[8];
+int Twsh;
+int mode =0;
+int cross=0;//十字数える用
+int cros=0;
+int countss=100000;//wait代替え
+
+///*********PWM調整用*********///
+int fast =60;
+int normal = 40;
+int slow = 20;
+
+bool MU_flag = false;
+
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
 #if USE_PROCESS_NUM>0
 static void Process0(void);
+//手動モード
 #endif
 #if USE_PROCESS_NUM>1
 static void Process1(void);
@@ -176,32 +252,163 @@
 void SystemProcess()
 {
     SystemProcessInitialize();
-    
+
     while(1) {
+        //lock = false;
+        
+        for(int i = 0; i < 8; i++) {
+            g[i] = LineHub::GetPara(i);
+        }
+        
+        if(EMC==0) {
+            fast=40;
+            normal=30;
+            slow=20;
+            cross=0;//十字数える用
+            cros=0;
+            countss=0;//wait代替え
+            UP_flag = false;
+            SW_flag = false;
+            Air_flag = false;
+            zyouge=false;
+            zyougedo=false;
+            dz1=true;
+            dz1i=false;
+            dz2=true;
+            dz2i=false;
+            dz3=true;
+            dz3i=false;
+            dz4=true;
+            dz4i=false;
+            through=false;
+            counts=false;
+            mtc=false;
+            mtc2 = false;
+            current = 0;
+            SystemProcessInitialize();
+            if(emcs==false) {
+                AllActuatorReset();
+                emcs=true;
+            }
+        } else {
+            emcs=false;
+        }
 
-		#ifdef USE_MU
+        if(mode==0) {
+            if(ZoneSW) {
+                LedMode(1);
+            } else {
+                LedMode(2);
+            }
+        }
+        for(int i = 0; i < 8; i++) {
+            g[i] = LineHub::GetPara(i);
+        }
+        if(ThSW) {
+            Twsh=1;
+        } else {
+            Twsh=2;
+        }
+
+        if(StertSW && start_flag == true) {
+            start_flag = false;
+            lock = false;
+            if(ZoneSW) {
+                countss=0;
+                mode=1;
+                current = 6;
+
+            } else {
+                countss=0;
+                mode=1;
+                current = 4;
+            }
+        }
+        if(DWSW||DOSW) {
+            if(DWSW) {
+                zyouge=true;
+            }
+            if(DOSW) {
+                if(DOLS) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                } else {
+                    motor[MOTOR_0].pwm = 50;
+                    motor[MOTOR_0].dir = BACK;
+                }
+            }
+        } else if(zyouge==false) {
+            motor[MOTOR_0].pwm = 100;
+            motor[MOTOR_0].dir = BRAKE;
+        }
+        if(zyouge) {
+            if(Twsh==1) {
+                motor[MOTOR_0].pwm = 215;
+                motor[MOTOR_0].dir = FOR;
+                if(UPLS) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                    zyouge=false;
+                }
+            }
+            if(Twsh==2) {
+                motor[MOTOR_0].pwm = 245;
+                motor[MOTOR_0].dir = FOR;
+                if(UPLS2) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                    zyouge=false;
+                }
+            }
+        }
+        if(AIRSW) {
+            if(SW_flag==false) {
+                if(Air_flag==false) {
+                    solenoid.solenoid3 = SOLENOID_ON;
+                    Air_flag=true;
+                    SW_flag=true;
+                } else {
+                    solenoid.solenoid3 = SOLENOID_OFF;
+                    Air_flag=false;
+                    SW_flag=true;
+                }
+            }
+        } else {
+            if(SW_flag==true) {
+                countss++;
+                if(countss>=2000) {
+                    SW_flag=false;
+                    countss=0;
+                }
+            }
+        }
+
+
+#ifdef USE_MU
         controller = CONTROLLER::Controller::GetData();
-		#endif
+#endif
+
 
-		#ifdef USE_ERRORCHECK
-        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
+#ifdef USE_ERRORCHECK
+        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
             CONTROLLER::Controller::DataReset();
             AllActuatorReset();
             lock = true;
         } else
-		#endif
+    
+#endif
         {
 
-			#ifdef USE_SUBPROCESS
+#ifdef USE_SUBPROCESS
             if(!lock) {
                 Process[current]();
             } else
-			#endif
+#endif
             {
                 //ロック時の処理
             }
         }
-        
+
         SystemProcessUpdate();
     }
 }
@@ -214,35 +421,1813 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-    AllActuatorReset();
+
+    //AllActuatorReset();
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
+    LedMode(6);
+    start_flag = true;
+    mtc=false;
+    mtc2 = false;
+    mode=1;
+    cross=0;
+    Twsh=1;
+    motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
+    motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
+    motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
+    motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
 
+    motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+    motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+    motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+    motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+
+    if(controller->Button.UP||controller->Button.DOWN) {
+
+        if(controller->Button.UP) {
+            motor[MOTOR_0].pwm =200;
+            motor[MOTOR_0].dir = FOR;
+            if(UPLS) {
+                motor[MOTOR_0].pwm = 0;
+                motor[MOTOR_0].dir = BRAKE;
+            }
+        }
+        if(controller->Button.DOWN) {
+            motor[MOTOR_0].pwm = 50;
+            motor[MOTOR_0].dir = BACK;
+        }
+    } else {
+        motor[MOTOR_0].pwm = 0;
+        motor[MOTOR_0].dir = BRAKE;
+    }
+
+    if(controller->Button.Y) {
+        if(dz2==true) {
+            if(dz2i==false) {
+                solenoid.solenoid2 = SOLENOID_ON;
+                dz2i=true;
+            } else {
+                solenoid.solenoid2 = SOLENOID_OFF;
+                dz2i=false;
+            }
+            dz2=false;
+        }
+    } else {
+        dz2=true;
+    }
+
+    if(controller->Button.A) {
+        if(dz3==true) {
+            if(dz3i==false) {
+
+                dz3i=true;
+            } else {
+                solenoid.solenoid3 = SOLENOID_OFF;
+                dz3i=false;
+            }
+            dz3=false;
+        }
+    } else {
+        dz3=true;
+    }
+
+    if(controller->Button.B) {
+        if(dz4==true) {
+            if(dz4i==false) {
+                solenoid.solenoid4 = SOLENOID_ON;
+                dz4i=true;
+            } else {
+                solenoid.solenoid4 = SOLENOID_OFF;
+                dz4i=false;
+            }
+            dz4=false;
+        }
+    } else {
+        dz4=true;
+    }
 }
 #endif
 
 #if USE_PROCESS_NUM>2
 static void Process2()
 {
+    //Oh So 青ゾーン
+    countss++;
+    LedMode(4);
+    if(mode==1) {//スタートゾーンから白線検知
+        fast=40;
+        normal=30;
+        slow=20;
+        motor[TIRE_FR].pwm = 27;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 33;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 33;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 27;
+        motor[TIRE_BL].dir = BACK;
+        //スタート後どれくらい無検知でうごくか↓
+        if(g[2]==0&&countss>=100000) {
+            countss=100000;
+            cross=0;
+            mode=3;
+        }
+    } else if(mode==3) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==0) {
+            mode=10;
+            countss=100000;
+        }
+    } else if(mode==10) { //縦ライントレース
 
+        if(countss==false&&g[2]==0) {
+            cros++;
+            countss=true;
+        }
+        if(countss==true&&g[2]==99) {
+            countss=false;
+        }
+
+        if(cros==Twsh) {
+            mode=11;
+            countss=0;
+            cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        switch(g[1]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                if(countss>100000) {
+                    cross++;
+                    countss=0;
+                }
+                if(cross==Twsh) {
+                    mode=11;
+                    countss=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[0]!=98&&mtc==true) {
+            switch(g[0]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc=false;
+                    break;
+            }
+        }
+    } else if(mode==11) {
+        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].dir = BACK;
+        if(g[2]==0) {
+            countss=0;
+            cross=0;
+            mode=20;
+        }
+    } else if(mode==20) {
+
+        if(Twsh==2) {
+            solenoid.solenoid2 = SOLENOID_ON;
+        }
+
+        if(counts==false&&g[1]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[1]==99) {
+            counts=false;
+        }
+
+        if(cros==2) {
+            //mode=21;
+            //countss=0;
+            //cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        if(g[1]==0) {
+            fast=35;
+            normal=20;
+            slow=15;
+        }
+        switch(g[2]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                if(countss>50000) {
+                    cross++;
+                    countss=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=22;
+                    cross=0;
+                    countss=0;
+                    fast=40;
+                    normal=30;
+                    slow=20;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[3]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+
+    } else if(mode==21) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BRAKE;
+        if(countss<=100000) {
+            mode=22;
+        }
+    }
+    kiri=true;
+    if(kiri) {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 100;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 100;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 100;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 100;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=23;
+            }
+        } else if(mode==23) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]==98) {
+                mode=24;
+                countss=0;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]!=98) {
+                countss=0;
+                mode=25;
+            }
+        } else if(mode==25) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=26;
+                countss=0;
+            }
+        } else if(mode==26) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = BACK;
+            if(countss>=5000) {
+                mode=27;
+                countss=0;
+            }
+        } else if(mode==27) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            //どれくらい回転するかって値
+            if(countss>=4000) {
+                countss=0;
+                mode=28;
+            }
+        } else if(mode==28) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=30;
+                countss=0;
+            }
+        }
+    } else {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 0;
+            motor[TIRE_FR].dir = FREE;
+            motor[TIRE_FL].pwm = 25;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 25;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 0;
+            motor[TIRE_BL].dir = FREE;
+            if(g[0]==0&&countss>=10000) {
+                mode=30;
+                countss=0;
+                cros=0;
+            }
+        }
+    }
+    if(mode==30) {
+        motor[TIRE_FR].pwm = 60;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 60;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 60;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 57;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==98) {
+            mode=31;
+            countss=0;
+        }
+    } else if(mode==31) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(countss>=5000) {
+            mode=32;
+            countss=0;
+        }
+    } else if(mode==32) {
+        motor[TIRE_FR].pwm = 10;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 10;
+        motor[TIRE_BL].dir = BRAKE;
+        if(countss>=30000) {
+            mode=33;
+            countss=0;
+        }
+    } else if(mode==33) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BACK;
+        if(g[3]==0&&countss>=20000) {
+            countss=0;
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
+            mode=40;
+        }
+    } else if(mode==40) {
+        if(LIB) {
+            mode=42;
+        }
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                if(countss>=10000) {
+                    cross++;
+                    countss=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=41;
+                    countss=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_FR].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_FR].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_BR].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_BR].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+    } else if(mode==41) {
+        if(LIF||LIB) {
+            mode=42;
+        }
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = FOR;
+        if(g[1]==0) {
+            countss=0;
+            mode=70;
+            normal=60;
+            fast=70;
+            slow=40;
+        }
+    } else if(mode==42) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 20;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 20;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[0]==0) {
+            countss=0;
+            slow=40;
+        }
+
+    } else if(mode==70)
+        switch(g[0]) {
+            case 99:
+                if(countss>=100000) {
+                    cross++;
+                    mode=100;
+                    countss=0;
+                }
+                if(cross==2) {
+                    cross=0;
+                    mode=100;
+                }
+                break;
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+    if(g[1]!=98&&mtc==true) {
+        switch(g[1]) {
+            case 0:
+                mtc=false;
+                break;
+            case 255:
+                motor[TIRE_BR].pwm += 5;
+                mtc=false;
+                break;
+            case 253:
+                motor[TIRE_BR].pwm += 10;
+                mtc=false;
+                break;
+            case 254:
+                motor[TIRE_BR].pwm += 20;
+                motor[TIRE_BL].pwm = 0;
+                mtc=false;
+                break;
+            case 1:
+                motor[TIRE_BL].pwm += 5;
+                mtc=false;
+                break;
+            case 3:
+                motor[TIRE_BL].pwm += 10;
+                mtc=false;
+                break;
+            case 2:
+                motor[TIRE_BL].pwm += 20;
+                motor[TIRE_BR].pwm = 0;
+                mtc=false;
+                break;
+        }
+    } else if(mode==100) {
+        motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 20;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 23;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = FOR;
+    }
 }
 #endif
 
 #if USE_PROCESS_NUM>3
 static void Process3()
 {
-    
+//Clearly 赤ゾーン
+    LedMode(3);
+    countss++;
+    if(mode==1) {//スタートゾーンから白線検知
+        fast=40;
+        normal=30;
+        slow=20;
+        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].dir = FOR;
+        if(g[2]==0&&countss>=100000) {
+            countss=100000;
+            cross=0;
+            mode=3;
+        }
+    } else if(mode==3) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 25;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 25;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[0]==0) {
+            mode=10;
+            countss=100000;
+        }
+    } else if(mode==10) { //縦ライントレース
+        if(counts==false&&g[2]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[2]==99) {
+            counts=false;
+        }
+
+        if(cros==Twsh) {
+            mode=11;
+            countss=0;
+            cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        switch(g[0]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                if(countss>80000) {
+                    cross++;
+                    countss=0;
+                }
+                if(cross==Twsh) {
+                    mode=11;
+                    countss=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[0]!=98&&mtc==true) {
+            switch(g[1]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_BR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_BR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
+                    mtc=false;
+                    break;
+            }
+        }
+    } else if(mode==11) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[2]==0) {
+            countss=0;
+            cross=0;
+            mode=20;
+        }
+    } else if(mode==20) {
+
+        if(Twsh==2) {
+            solenoid.solenoid2 = SOLENOID_ON;
+        }
+        if(counts==false&&g[0]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[0]==99) {
+            counts=false;
+        }
+
+        if(cros==2) {
+
+            // mode=21;
+            // countss=0;
+            // cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        if(g[0]==0) {
+            fast=35;
+            normal=20;
+            slow=15;
+        }
+        switch(g[2]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                if(countss>30000) {
+                    cross++;
+                    countss=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=22;
+                    countss=0;
+                    cross=0;
+                    fast=40;
+                    normal=30;
+                    slow=20;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[3]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+    } else if(mode==21) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BACK;
+        if(g[2]==98&&countss>=10000) {
+            mode=22;
+            countss=0;
+        }
+    }
+    kiri=true;
+    if(kiri) {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 100;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 100;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 100;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 100;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=23;
+            }
+        } else if(mode==23) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]==98) {
+                mode=24;
+                countss=0;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]!=98) {
+                countss=0;
+                mode=25;
+            }
+        } else if(mode==25) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=26;
+                countss=0;
+            }
+        } else if(mode==26) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = BACK;
+            if(countss>=5000) {
+                mode=27;
+                countss=0;
+            }
+        } else if(mode==27) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            //どれくらい回転するかって値
+            if(countss>=100) {
+                countss=0;
+                mode=30;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(countss>=10000) {
+                mode=30;
+                countss=0;
+            }
+        }
+    } else {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 0;
+            motor[TIRE_FR].dir = FREE;
+            motor[TIRE_FL].pwm = 25;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 25;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 0;
+            motor[TIRE_BL].dir = FREE;
+            if(g[0]==0&&countss>=10000) {
+                mode=30;
+                countss=0;
+                cros=0;
+            }
+        }
+    }
+    if(mode==30) {
+        motor[TIRE_FR].pwm = 59;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 62;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 59;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 62;
+        motor[TIRE_BL].dir = FOR;
+        if(g[0]==98) {
+            mode=31;
+            countss=0;
+        }
+    } else if(mode==31) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(countss>=5000) {
+            countss=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        motor[TIRE_FR].pwm = 10;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 10;
+        motor[TIRE_BL].dir = BRAKE;
+        if(countss>=40000) {
+            mode=33;
+            countss=0;
+        }
+    } else if(mode==33) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FOR;
+        if(g[3]==0&&countss>=20000) {
+            countss=0;
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
+            mode=40;
+        }
+    } else if(mode==40) {
+        if(LIB) {
+            mode=42;
+        }
+        if(counts==false&&g[0]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[0]==99) {
+            counts=false;
+        }
+
+        if(cros==1) {
+            // mode=41;
+            //countss=0;
+            //cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                if(countss>20000) {
+                    cross++;
+                    countss=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=41;
+                    countss=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+    } else if(mode==41) {
+        if(LIF||LIB) {
+            mode=42;
+        }
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[1]==0) {
+            countss=0;
+            mode=70;
+            normal=60;
+            fast=70;
+            slow=40;
+        }
+    } else if(mode==42) {
+        motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==0) {
+            countss=0;
+            mode=70;
+        }
+    } else if(mode==70) {
+        switch(g[1]) {
+            case 99:
+                if(countss>=100000) {
+                    mode=100;
+                }
+                break;
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 80;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 80;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 80;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 80;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[1]!=98&&mtc==true) {
+            switch(g[0]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc=false;
+                    break;
+            }
+        }
+    } else if(mode==100) {
+        motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 20;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 23;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = BACK;
+    } else {
+    }
+
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4()
 {
-    
+
 }
 #endif
 
@@ -263,14 +2248,14 @@
 #if USE_PROCESS_NUM>7
 static void Process7()
 {
-   
+
 }
 #endif
 
 #if USE_PROCESS_NUM>8
 static void Process8()
 {
-	
+
 }
 #endif
 
@@ -285,16 +2270,16 @@
 
 static void AllActuatorReset()
 {
-	#ifdef USE_SOLENOID
-	    solenoid.all = ALL_SOLENOID_OFF;
-	#endif
-	
-	#ifdef USE_MOTOR
-	    for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
-	        motor[i].dir = FREE;
-	        motor[i].pwm = 0;
-	    }
-	#endif
+#ifdef USE_SOLENOID
+    solenoid.all = ALL_SOLENOID_OFF;
+#endif
+
+#ifdef USE_MOTOR
+    for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+        motor[i].dir = FREE;
+        motor[i].pwm = 0;
+    }
+#endif
 }
 
 #pragma region USER-DEFINED-FUNCTIONS