aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
40:7f93984e8ba1
Parent:
39:f89166d7411c
Child:
41:e1dd6d97f17d
diff -r f89166d7411c -r 7f93984e8ba1 System/Process/Process.cpp
--- a/System/Process/Process.cpp	Sat Sep 28 02:49:10 2019 +0000
+++ b/System/Process/Process.cpp	Sat Sep 28 13:15:16 2019 +0000
@@ -188,9 +188,9 @@
 int count=100000;//wait代替え
 
 ///*********PWM調整用*********///
-int fast =80;
-int normal = 60;
-int slow = 40;
+int fast =60;
+int normal = 40;
+int slow = 20;
 
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
@@ -373,7 +373,7 @@
             current = 2;
         } else {
             mode=1;
-            current = 4;
+            current = 5;
         }
     }
     if(DWSW) {
@@ -1441,7 +1441,7 @@
         motor[TIRE_FL].dir = BACK;
         motor[TIRE_BR].pwm = 40;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 35;
+        motor[TIRE_BL].pwm = 45;
         motor[TIRE_BL].dir = BACK;
         if(g[2]==0) {
             count=100000;
@@ -1990,7 +1990,7 @@
             counts=false;
         }
 
-        if(cros==2) {
+        if(cros==1) {
             // mode=41;
             //count=0;
             //cros=0;
@@ -2289,50 +2289,968 @@
         if(StertSW) {
             SW_flag = true;
         }
-        if(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) {
+            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(5);
+    count++;
+    if(mode==1) {//スタートゾーンから白線検知
+        fast=40;
+        normal=30;
+        slow=20;
+        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 = 40;
+        motor[TIRE_BL].dir = BACK;
+        if(g[2]==0) {
+            count=100000;
+            cross=0;
+            mode=3;
+        }
+    } else if(mode==3) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = 25;
+        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].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==1) {
+                    fast=30;
+                    normal=20;
+                    slow=20;
+                }
+                if(cross==2) {
+                    mode=21;
+                    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 = 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=30;
+        }
+
+    } 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 = 60;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==98) {
+            mode=31;
+        }
+        //ゆっくりモードに入れなかった時の保険
+        /*
+        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==2) {
+            // 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==1) {
+                    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==42) {
+        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[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