aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
43:677f7446612e
Parent:
42:4f9689c0a349
Child:
44:c208a908583c
--- a/System/Process/Process.cpp	Mon Sep 30 11:33:22 2019 +0000
+++ b/System/Process/Process.cpp	Wed Oct 02 01:53:59 2019 +0000
@@ -124,6 +124,8 @@
     {1,1,1,1}
 };
 
+DigitalIn EMC(PB_0);
+
 void LedMode(int num)
 {
     POTENTIOMETER::dio[0]= selectnum[num][0];
@@ -151,6 +153,7 @@
 bool counts=false;
 bool mtc=false;
 bool mtc2 = false;
+bool emcs=false;
 
 bool start_flag = true;
 
@@ -280,6 +283,40 @@
     SystemProcessInitialize();
 
     while(1) {
+        if(EMC==0) {
+            fast=40;
+            normal=30;
+            slow=20;
+            cross=0;//十字数える用
+            cros=0;
+            count=100000;//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;
+        }
+
         if(mode==0) {
             if(ZoneSW) {
                 LedMode(1);
@@ -304,7 +341,7 @@
                 current = 2;
             } else {
                 mode=1;
-                current = 5;
+                current = 4;
             }
         }
         if(DWSW||DOSW) {
@@ -359,7 +396,7 @@
         } else {
             if( SW_flag==true) {
                 count++;
-                if(count>=5000) {
+                if(count>=2000) {
                     SW_flag=false;
                     count=0;
                 }
@@ -390,20 +427,7 @@
                 //ロック時の処理
             }
         }
-        /*
-        //Emergency!
-        if(!EMG_0 && !EMG_1 && !EMGflag) {
-            buzzer = 0;
-            BuzzerTimer.attach(BuzzerTimer_func, 1);
-            EMGflag = true;
-            LED_DEBUG0 = 1;
-        }
-        if(EMG_0 && EMG_1 && EMGflag) {
-            buzzer = 1;
-            BuzzerTimer.detach();
-            EMGflag = false;
-        }
-        */
+
         SystemProcessUpdate();
     }
 }
@@ -486,7 +510,6 @@
     }
     */
 }
-
 #endif
 
 #if USE_PROCESS_NUM>1
@@ -823,8 +846,8 @@
                     count=0;
                 }
                 mtc2=true;
-                if(cross==1) {
-                    mode=21;
+                if(cross==2) {
+                    mode=22;
                     count=0;
                     cross=0;
                 }
@@ -941,11 +964,11 @@
             }
         }
     } else if(mode==21) {
-        motor[TIRE_FR].pwm = 26;
+        motor[TIRE_FR].pwm = 30;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 30;
         motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 26;
+        motor[TIRE_BR].pwm = 30;
         motor[TIRE_BR].dir = BACK;
         motor[TIRE_BL].pwm = 30;
         motor[TIRE_BL].dir = BACK;
@@ -953,46 +976,88 @@
             mode=22;
             count=0;
         }
-    } else if(mode==22) {
-        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) {
-            count=0;
-            mode=23;
+    }
+    if(YELLOW) {
+        if(mode==22) {
+            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(count<=100000) {
+                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) {
+                count=0;
+                mode=24;
+            }
+        } 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(count<=100000) {
+                mode=25;
+            }
+        } else if(mode==25) {
+            motor[TIRE_FR].pwm = 15;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 15;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 15;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 15;
+            motor[TIRE_BL].dir = FOR;
+            if(count<=100000) {
+                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(count<=100000) {
+                mode=30;
+            }
         }
-    } else if(mode==23) {
-        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(count<=100000) {
-            mode=30;
+    } 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) {
+                mode=30;
+                cros=0;
+            }
         }
-    } else if(mode==24) {
-        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[0]==0) {
-            mode=30;
-            cros=0;
-        }
-    } else if(mode==30) {
-        motor[TIRE_FR].pwm = 63;
+    }
+    if(mode==30) {
+        motor[TIRE_FR].pwm = 60;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = 60;
         motor[TIRE_FL].dir = FOR;
@@ -1363,14 +1428,6 @@
         motor[TIRE_BL].pwm = 20;
         motor[TIRE_BL].dir = BACK;
     } else {
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = BRAKE;
     }
 }
 #endif
@@ -1378,7 +1435,6 @@
 #if USE_PROCESS_NUM>3
 static void Process3()
 {
-
 }
 #endif
 
@@ -1388,910 +1444,6 @@
     LedMode(4);
     count++;
     if(mode==1) {//スタートゾーンから白線検知
-        motor[TIRE_FR].pwm = 40;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 40;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 40;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 45;
-        motor[TIRE_BL].dir = BACK;
-        if(g[2]==0) {
-            count=100000;
-            cross=0;
-            mode=3;
-        }
-    } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 40;
-        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 = 40;
-        motor[TIRE_BL].dir = BACK;
-        if(g[0]==0) {
-            mode=10;
-            count=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;
-            count=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(count>80000) {
-                    cross++;
-                    count=0;
-                }
-                if(cross==Twsh) {
-                    mode=11;
-                    count=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) {
-            count=0;
-            cross=0;
-            mode=20;
-            fast=40;
-            normal=30;
-            slow=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;
-            //count=0;
-            //cros=0;
-        }//ゆっくりモードに入れなかった時の保険
-
-        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(count>80000) {
-                    cross++;
-                    count=0;
-                }
-                mtc2=true;
-                if(cross==2) {
-                    mode=21;
-                    count=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 = 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 = 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[1]==0) {
-            mode=30;
-            cros=0;
-            zyouge=true;
-            fast=80;
-            normal=60;
-            slow=40;
-        }
-    } else 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 = 50;
-        motor[TIRE_BL].dir = BACK;
-        if(g[1]==98) {
-            mode=31;
-        }
-        if(g[3]==0) {
-            mode=32;
-            count=0;
-            cros=0;
-        }//ゆっくりモードに入れなかった時の保険
-        /*
-        switch(g[1]) {
-            case 98:
-                motor[TIRE_FR].pwm = 30;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 30;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 30;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 30;
-                motor[TIRE_BL].dir = BACK;
-                if(count>100000) {
-                    cross++;
-                    count=0;
-                }
-                mtc=true;
-                if(cross==1) {
-                    if(Twsh==2) {
-                        solenoid.solenoid2 = SOLENOID_OFF;
-                    }
-                    mode=31;
-                    count=0;
-                }
-                break;
-            case 0:
-                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 = 60;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = 60;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 20;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 20;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = 20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 20;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 254:
-                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;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = 20;
-                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 = 20;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = 20;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 20;
-                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 = 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;
-                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==31) {
-        motor[TIRE_FR].pwm = 15;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 15;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 15;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 15;
-        motor[TIRE_BL].dir = BACK;
-        if(g[3]==0) {
-            count=0;
-            mode=32;
-        }
-    } else if(mode==32) {
-        motor[TIRE_FR].pwm = 30;
-        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 = 30;
-        motor[TIRE_BL].dir = FOR;
-        if(g[3]==0) {
-            count=0;
-            if(Twsh==2) {
-                solenoid.solenoid2 = SOLENOID_OFF;
-            }
-            mode=40;
-        }
-    } else if(mode==40) {
-
-        if(LIF) {
-            mode=42;
-        }
-
-        if(counts==false&&g[0]==0) {
-            cros++;
-            counts=true;
-        }
-        if(counts==true&&g[0]==99) {
-            counts=false;
-        }
-
-        if(cros==1) {
-            // mode=41;
-            //count=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(count>20000) {
-                    cross++;
-                    count=0;
-                }
-                mtc2=true;
-                if(cross==2) {
-                    mode=41;
-                    count=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) {
-        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) {
-            count=0;
-            mode=70;
-            normal=60;
-            fast=80;
-            slow=40;
-        }
-    } else if(mode==70)
-        switch(g[0]) {
-            case 99:
-                if(count>=100000) {
-                    cross++;
-                    mode=100;
-                    count=0;
-                }
-                if(cross==3) {
-                    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;
-        if(StertSW) {
-            SW_flag = true;
-        }
-        if(SW_flag ==true) {
-            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;
-            fast=60;
-            normal=40;
-            slow=20;
-            mode =1;
-            cross=0;//十字数える用
-            cros=0;
-            count=100000;//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;
-            if(StertSW==false) {
-                SW_flag = false;
-            }
-        }
-    }
-
-
-}
-#endif
-
-#if USE_PROCESS_NUM>5
-static void Process5()
-{
-    LedMode(4);
-    count++;
-    if(mode==1) {//スタートゾーンから白線検知
         fast=50;
         normal=30;
         slow=20;
@@ -2301,7 +1453,7 @@
         motor[TIRE_FL].dir = BACK;
         motor[TIRE_BR].pwm = 35;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 32;
+        motor[TIRE_BL].pwm = 35;
         motor[TIRE_BL].dir = BACK;
         if(g[2]==0) {
             count=100000;
@@ -2309,13 +1461,13 @@
             mode=3;
         }
     } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 27;
+        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 = 25;
+        motor[TIRE_BL].pwm = 30;
         motor[TIRE_BL].dir = BACK;
         if(g[0]==0) {
             mode=10;
@@ -2664,10 +1816,21 @@
             motor[TIRE_BL].pwm = 20;
             motor[TIRE_BL].dir = FOR;
             if(g[2]!=98) {
-                mode=30;
+                mode=23;
             }
         }
-
+    } else if(mode==23) {
+        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(count<=100000) {
+            mode=30;
+        }
     } else {
         if(mode==22) {
             motor[TIRE_FR].pwm = normal;
@@ -2686,7 +1849,7 @@
         }
     }
     if(mode==30) {
-        motor[TIRE_FR].pwm = 64;
+        motor[TIRE_FR].pwm = 60;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 60;
         motor[TIRE_FL].dir = BACK;
@@ -3072,48 +2235,13 @@
         if(StertSW) {
             SW_flag = true;
         }
-        if(SW_flag ==true) {
-            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;
-            fast=60;
-            normal=40;
-            slow=20;
-            mode =1;
-            cross=0;//十字数える用
-            cros=0;
-            count=100000;//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;
-            if(StertSW==false) {
-                SW_flag = false;
-            }
-        }
     }
-
+}
+#endif
 
-
+#if USE_PROCESS_NUM>5
+static void Process5()
+{
 }
 #endif