aa

Dependencies:   mbed TrapezoidControl QEI

Revision:
31:45f97698857b
Parent:
30:9450dc1d53cc
Child:
32:90f018b10ad6
--- a/System/Process/Process.cpp	Fri Sep 20 02:47:02 2019 +0000
+++ b/System/Process/Process.cpp	Sat Sep 21 04:28:31 2019 +0000
@@ -104,6 +104,25 @@
     else return abs(pwmVal);
 }
 
+int Twsh;
+bool UP_flag = false;
+bool Air_flag = false;
+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;
+
+int mode =1;
+int cross=0;//十字数える用
+int count=100000;//wait代替え
+int slow=30;
+
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
@@ -264,46 +283,72 @@
     }
 }
 
-
-int Twsh;
-
 #pragma region PROCESS
 #ifdef USE_SUBPROCESS
 #if USE_PROCESS_NUM>0
 
 static void Process0()
 {
+    for(int i=0; i<=3; i++) {
+        if(i==0) {
+            POTENTIOMETER::dio[0]= 1;
+        } else {
+            POTENTIOMETER::dio[i]=0;
+        }
+    }
     if(StertSW) {
-        current = 2;
-        Twsh=3;
+        if(ZoneSW) {
+            if(ThSW) {
+                Twsh=1;
+            } else {
+                Twsh=2;
+            }
+            mode=1;
+            current = 2;
+        } else {
+            if(ThSW) {
+                Twsh=1;
+            } else {
+                Twsh=2;
+            }
+            mode=1;
+            current = 3;
+        }
     }
+
+    if(UPSW) {
+        UP_flag = true;
+    }
+
+    if(UP_flag) {
+        motor[MOTOR_0].dir = FOR;
+        motor[MOTOR_0].pwm = 100;
+
+        if (UPLS) {
+            motor[MOTOR_0].dir = BRAKE;
+            UP_flag = false;
+            Air_flag = true;
+        }
+    }
+    if(Air_flag) {
+        solenoid.solenoid2 = SOLENOID_ON;
+    }
+
 }
 #endif
 
 #if USE_PROCESS_NUM>1
-
-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;
-
-int mode =1;
-int cross=0;//十字数える用
-int count=100000;//wait代替え
-int slow=30;
-
-
 static void Process1()
 {
+    for(int i=0; i<=3; i++) {
+        if(i==1) {
+            POTENTIOMETER::dio[1]=1;
+        } else {
+            POTENTIOMETER::dio[i]=0;
+        }
+    }
     count=100000;
-    mode=0;
+    mode=1;
     cross=0;
     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]);
@@ -414,41 +459,241 @@
 
 static void Process2()
 {
- //   printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
-    count++;
-    if(mode==0&&StertSW) {
-        if(ThSW) {
-            zone=ZoneSW;
-            Twsh=1;
-            mode=1;
+    for(int i=0; i<=3; i++) {
+        if(i==2) {
+            POTENTIOMETER::dio[2]=1;
         } else {
-            zone=ZoneSW;
-            Twsh=2;
-            mode=1;
+            POTENTIOMETER::dio[i]=0;
         }
     }
-    if(ZoneSW==false) {
-        if(mode==1) {//スタートゾーンから白線検知
+//   printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
+    count++;
+    if(mode==1) {//スタートゾーンから白線検知
+        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[2]==0) {
+            count=100000;
+            cross=0;
+            mode=2;
+        }
+    } else if(mode==2) {//横移動
+        if(g[2]==0) {
+            count=100000;
+            cross=0;
+            mode=3;
+            mtc2=true;
+        } else {
             motor[TIRE_FR].pwm = 30;
-            motor[TIRE_FR].dir = BACK;
+            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 = FOR;
-            if(g[2]==0) {
-                count=100000;
-                cross=0;
-                mode=2;
+            motor[TIRE_BL].dir = BACK;
+        }
+    } else if(mode==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;
+        if(g[0]==0) {
+            through=true;
+        }
+        if(through==true&&g[0]==255) {
+            through=false;
+            count=0;
+            mode=4;
+        }
+    } else if(mode==4) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = slow;
+        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 = slow;
+        motor[TIRE_BL].dir = FOR;
+        if(g[0]==0) {
+            count=0;
+            cross=0;
+            mode=10;
+        }
+    } else if(g[0]!=99&&mode==10) { //縦ライントレース
+        switch(g[0]) {
+            case 98:
+                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(count>100000) {
+                    cross++;
+                    count=0;
+                }
+                mtc=true;
+                if(cross==Twsh) {
+                    mode=11;
+                    count=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                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;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                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;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = 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==2) {//横移動
-            if(g[2]==0) {
-                count=100000;
-                cross=0;
-                mode=3;
-                mtc2=true;
-            } else {
+        }
+    } else if(mode==11) {
+        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;
+        if(g[2]==0) {
+            through=true;
+        }
+        if(through==true&&g[2]==255) {
+            through=false;
+            mode=12;
+        }
+    } else if(mode==12) {
+        motor[TIRE_FR].pwm = slow;
+        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 = slow;
+        motor[TIRE_BL].dir = BACK;
+        if(g[2]==0) {
+            count=0;
+            cross=0;
+            mode=20;
+        }
+    } else if(mode==20) {
+        switch(g[2]) {
+            case 98:
                 motor[TIRE_FR].pwm = 30;
                 motor[TIRE_FR].dir = FOR;
                 motor[TIRE_FL].pwm = 30;
@@ -457,1170 +702,1176 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = 30;
                 motor[TIRE_BL].dir = BACK;
-            }
-        } else if(mode==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;
-            if(g[0]==0) {
-                through=true;
-            }
-            if(through==true&&g[0]==255) {
-                through=false;
-                count=0;
-                mode=4;
-            }
-        } else if(mode==4) { //横ライントレースから縦ライントレースへ
-            motor[TIRE_FR].pwm = slow;
-            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 = slow;
-            motor[TIRE_BL].dir = FOR;
-            if(g[0]==0) {
-                count=0;
-                cross=0;
-                mode=10;
-            }
-        } else if(g[0]!=99&&mode==10) { //縦ライントレース
-            switch(g[0]) {
-                case 98:
-                    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(count>100000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc=true;
-                    if(cross==Twsh) {
-                        mode=11;
-                        count=0;
-                    }
-                    break;
+                if(count>70000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==3) {
+                    mode=21;
+                    count=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 3:
+                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;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[3]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_BL].pwm += 20;
                     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;
-                    mtc=true;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
-                    break;
-                case 3:
-                    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;
-                    mtc=true;
-                    break;
-                case 2:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = 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 = 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;
-            if(g[2]==0) {
-                through=true;
-            }
-            if(through==true&&g[2]==255) {
-                through=false;
-                mode=12;
-            }
-        } else if(mode==12) {
-            motor[TIRE_FR].pwm = slow;
-            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 = slow;
-            motor[TIRE_BL].dir = BACK;
-            if(g[2]==0) {
-                count=0;
-                cross=0;
-                mode=20;
-            }
-        } else if(mode==20) {
-            switch(g[2]) {
-                case 98:
-                    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(count>70000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==3) {
-                        mode=21;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
-                case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
-                    break;
-                case 253:
-                    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;
-                    mtc2=true;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = FREE;
-                    mtc2=true;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 3:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
                     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 = 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;
+        if(g[0]==0) {
+            through=true;
+        }
+        if(through==true&&g[0]==255) {
+            through=false;
+            mode=22;
+        }
+    } else if(mode==22) {
+        motor[TIRE_FR].pwm = slow;
+        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 = slow;
+        motor[TIRE_BL].dir = FOR;
+        if(g[0]==0) {
+            solenoid.solenoid3 = SOLENOID_ON;
+            count=0;
+            cross=0;
+            mode=30;
+        }
+    } else if(mode==30) {
+        switch(g[0]) {
+            case 98:
+                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(count>100000) {
+                    cross++;
+                    count=0;
+                }
+                mtc=true;
+                if(cross==1) {
+                    solenoid.solenoid3 = SOLENOID_OFF;
+                    mode=31;
+                    count=0;
                 }
-            }
-        } else if(mode==21) {
-            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;
-            if(g[0]==0) {
-                through=true;
-            }
-            if(through==true&&g[0]==255) {
-                through=false;
-                mode=22;
-            }
-        } else if(mode==22) {
-            motor[TIRE_FR].pwm = slow;
-            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 = slow;
-            motor[TIRE_BL].dir = FOR;
-            if(g[0]==0) {
-                solenoid.solenoid3 = SOLENOID_ON;
-                count=0;
-                cross=0;
-                mode=30;
-            }
-        } else if(mode==30) {
-            switch(g[0]) {
-                case 98:
-                    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(count>100000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc=true;
-                    if(cross==1) {
-                        solenoid.solenoid3 = SOLENOID_OFF;
-                        mode=31;
-                        count=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                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;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                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;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[0]!=98&&mtc==true) {
+            switch(g[1]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_BR].pwm += 5;
+                    mtc=false;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_BR].pwm += 10;
+                    mtc=false;
                     break;
                 case 254:
-                    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;
-                    mtc=true;
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc=false;
                     break;
                 case 3:
-                    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;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc=false;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = FREE;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
+                    mtc=false;
                     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==31) {
+        motor[TIRE_FR].pwm = 15;
+        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 = 15;
+        motor[TIRE_BL].dir = FOR;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        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(count>5000&&g[3]==0) {
+            count=0;
+            cross=0;
+            mode=40;
+        }
+    } else if(mode==40) {
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = 30;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                if(count>30000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==1) {
+                    mode=41;
+                    count=0;
+                    cross=0;
                 }
-            }
-        } else if(mode==31) {
-            motor[TIRE_FR].pwm = 15;
-            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 = 15;
-            motor[TIRE_BL].dir = FOR;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                mode=32;
-            }
-        } else if(mode==32) {
-            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(count>5000&&g[3]==0) {
-                count=0;
-                cross=0;
-                mode=40;
-            }
-        } else if(mode==40) {
-            switch(g[3]) {
-                case 98:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    if(count>30000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==1) {
-                        mode=41;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                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;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    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_FL].pwm += 20;
                     motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_BL].pwm += 20;
                     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;
-                    mtc2=true;
+                    mtc2=false;
                     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) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=42;
+        }
+    } else if(mode==42) {
+        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 = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = BACK;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=50;
+        }
+    } else if(mode==50) {
+        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) {
+                    mode=51;
+                    count=0;
                 }
-            }
-        } else if(mode==41) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=42;
-            }
-        } else if(mode==42) {
-            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 = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = BACK;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=50;
-            }
-        } else if(mode==50) {
-            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) {
-                        mode=51;
-                        count=0;
-                    }
-                    break;
+                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 = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                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 = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                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 = 40;
+                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 = 40;
+                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:
-                    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;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    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 = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
                     break;
                 case 253:
-                    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;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
                     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;
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    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 = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
                     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;
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
                     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;
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc=false;
                     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==51) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            mode=52;
+        }
+    } else if(mode==52) {
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = FOR;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            cross=0;
+            mode=60;
+        }
+    } else if(mode==60) {
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = 30;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                if(count>50000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=61;
+                    count=0;
+                    cross=0;
                 }
-            }
-        } else if(mode==51) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                mode=52;
-            }
-        } else if(mode==52) {
-            motor[TIRE_FR].pwm = 15;
-            motor[TIRE_FR].dir = BACK;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = FOR;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = FOR;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                cross=0;
-                mode=60;
-            }
-        } else if(mode==60) {
-            switch(g[3]) {
-                case 98:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    if(count>50000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==2) {
-                        mode=61;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                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;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    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_FL].pwm += 20;
                     motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_BL].pwm += 20;
                     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;
-                    mtc2=true;
+                    mtc2=false;
                     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==61) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=62;
+        }
+    } else if(mode==62) {
+        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 = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = BACK;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=70;
+        }
+    } else if(mode==70) {
+        switch(g[1]) {
+            case 99:
+                if(count>=100000) {
+                    mode=100;
                 }
-            }
-        } else if(mode==61) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=62;
-            }
-        } else if(mode==62) {
-            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 = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = BACK;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=70;
-            }
-        } else if(mode==70) {
-            switch(g[1]) {
-                case 99:
-                    if(count>=100000) {
-                        mode=100;
-                    }
-                    break;
-                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;
-                    break;
+                break;
+            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;
+                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 = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                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 = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                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 = 40;
+                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 = 40;
+                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:
-                    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;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    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 = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
                     break;
                 case 253:
-                    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;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
                     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;
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    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 = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
                     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;
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
                     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;
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc=false;
                     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 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 {
+        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
+
+#if USE_PROCESS_NUM>3
+static void Process3()
+{
+    for(int i=0; i<=3; i++) {
+        if(i==3) {
+            POTENTIOMETER::dio[3]=1;
         } 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;
+            POTENTIOMETER::dio[i]=0;
         }
-    } else {//////////////////////////////////////////////////これより別ゾーン用/////////////////////////////////////////////////////////////////////////////////
-        if(mode==1) {//スタートゾーンから白線検知
+    }
+    if(mode==1) {//スタートゾーンから白線検知
+        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(g[2]==0) {
+            count=100000;
+            cross=0;
+            mode=2;
+        }
+    } else if(mode==2) {//横移動
+        if(g[2]==0) {
+            count=100000;
+            cross=0;
+            mode=3;
+            mtc2=true;
+        } else {
             motor[TIRE_FR].pwm = 30;
             motor[TIRE_FR].dir = FOR;
             motor[TIRE_FL].pwm = 30;
-            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_FL].dir = FOR;
             motor[TIRE_BR].pwm = 30;
-            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BR].dir = BACK;
             motor[TIRE_BL].pwm = 30;
             motor[TIRE_BL].dir = BACK;
-            if(g[2]==0) {
-                count=100000;
-                cross=0;
-                mode=2;
+        }
+    } else if(mode==3) { //横ライントレースから縦ライントレースへ
+        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(count>100000&&g[0]==0) {
+            count=0;
+            mode=10;
+        }
+    } else if(mode==4) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = BACK;
+        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 = FOR;
+        if(count>30000&&g[0]==0) {
+            count=0;
+            cross=0;
+            mode=10;
+        }
+    } else if(g[0]!=99&&mode==10) { //縦ライントレース
+        switch(g[0]) {
+            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==Twsh) {
+                    mode=11;
+                    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 = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                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 = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                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 = 40;
+                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 = 40;
+                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[0]!=98&&mtc==true) {
+            switch(g[1]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_BR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_BR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc=false;
+                    break;
             }
-        } else if(mode==2) {//横移動
-            if(g[2]==0) {
-                count=100000;
-                cross=0;
-                mode=3;
-                mtc2=true;
-            } else {
+        }
+    } else if(mode==11) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[2]==0) {
+            count=0;
+            mode=12;
+        }
+    } else if(mode==12) {
+        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(count>10000&&g[2]==0) {
+            count=0;
+            cross=0;
+            mode=20;
+        }
+    } else if(mode==20) {
+        switch(g[2]) {
+            case 98:
                 motor[TIRE_FR].pwm = 30;
                 motor[TIRE_FR].dir = FOR;
                 motor[TIRE_FL].pwm = 30;
@@ -1629,1149 +1880,942 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = 30;
                 motor[TIRE_BL].dir = BACK;
-            }
-        } else if(mode==3) { //横ライントレースから縦ライントレースへ
-            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(count>100000&&g[0]==0) {
-                count=0;
-                mode=10;
-            }
-        } else if(mode==4) { //横ライントレースから縦ライントレースへ
-            motor[TIRE_FR].pwm = 15;
-            motor[TIRE_FR].dir = BACK;
-            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 = FOR;
-            if(count>30000&&g[0]==0) {
-                count=0;
-                cross=0;
-                mode=10;
-            }
-        } else if(g[0]!=99&&mode==10) { //縦ライントレース
-            switch(g[0]) {
-                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==Twsh) {
-                        mode=11;
-                        count=0;
-                    }
-                    break;
+                if(count>70000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==3) {
+                    mode=21;
+                    count=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 3:
+                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;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[3]) {
                 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;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    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 = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    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;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_BL].pwm += 20;
                     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;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    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 = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     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[0]!=98&&mtc==true) {
-                switch(g[1]) {
-                    case 0:
-                        mtc=false;
-                        break;
-                    case 255:
-                        motor[TIRE_BL].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 253:
-                        motor[TIRE_BL].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 254:
-                        motor[TIRE_BL].pwm += 20;
-                        motor[TIRE_BR].pwm = 0;
-                        mtc=false;
-                        break;
-                    case 1:
-                        motor[TIRE_BR].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 3:
-                        motor[TIRE_BR].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 2:
-                        motor[TIRE_BR].pwm += 20;
-                        motor[TIRE_BL].pwm = 0;
-                        mtc=false;
-                        break;
-                }
-            }
-        } else if(mode==11) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = FOR;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = BACK;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[2]==0) {
-                count=0;
-                mode=12;
-            }
-        } else if(mode==12) {
-            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(count>10000&&g[2]==0) {
-                count=0;
-                cross=0;
-                mode=20;
-            }
-        } else if(mode==20) {
-            switch(g[2]) {
-                case 98:
-                    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(count>70000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==3) {
-                        mode=21;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
-                case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
-                    break;
-                case 253:
-                    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;
-                    mtc2=true;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = FREE;
-                    mtc2=true;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc2=true;
-                    break;
-                case 3:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
                     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 = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>2000&&g[0]==0) {
+            count=0;
+            mode=22;
+        }
+    } else if(mode==22) {
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = BACK;
+        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 = FOR;
+        if(count>5000&&g[0]==0) {
+            count=0;
+            cross=0;
+            mode=30;
+        }
+    } else if(mode==30) {
+        switch(g[0]) {
+            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) {
+                    mode=31;
+                    count=0;
                 }
-            }
-        } else if(mode==21) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = FOR;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = BACK;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>2000&&g[0]==0) {
-                count=0;
-                mode=22;
-            }
-        } else if(mode==22) {
-            motor[TIRE_FR].pwm = 15;
-            motor[TIRE_FR].dir = BACK;
-            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 = FOR;
-            if(count>5000&&g[0]==0) {
-                count=0;
-                cross=0;
-                mode=30;
-            }
-        } else if(mode==30) {
-            switch(g[0]) {
-                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) {
-                        mode=31;
-                        count=0;
-                    }
-                    break;
+                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 = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                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 = 40;
+                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 = 40;
+                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[0]!=98&&mtc==true) {
+            switch(g[1]) {
                 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;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    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 = 60;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc=false;
                     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;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc=false;
                     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_BL].pwm += 20;
                     motor[TIRE_BR].pwm = 0;
-                    motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = 30;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    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 = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
+                    motor[TIRE_BR].pwm += 5;
+                    mtc=false;
                     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;
+                    motor[TIRE_BR].pwm += 10;
+                    mtc=false;
                     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_BR].pwm += 20;
                     motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc=true;
+                    mtc=false;
                     break;
             }
-            if(g[0]!=98&&mtc==true) {
-                switch(g[1]) {
-                    case 0:
-                        mtc=false;
-                        break;
-                    case 255:
-                        motor[TIRE_BL].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 253:
-                        motor[TIRE_BL].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 254:
-                        motor[TIRE_BL].pwm += 20;
-                        motor[TIRE_BR].pwm = 0;
-                        mtc=false;
-                        break;
-                    case 1:
-                        motor[TIRE_BR].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 3:
-                        motor[TIRE_BR].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 2:
-                        motor[TIRE_BR].pwm += 20;
-                        motor[TIRE_BL].pwm = 0;
-                        mtc=false;
-                        break;
+        }
+    } else if(mode==31) {
+        motor[TIRE_FR].pwm = 15;
+        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 = 15;
+        motor[TIRE_BL].dir = FOR;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        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(count>5000&&g[3]==0) {
+            count=0;
+            cross=0;
+            mode=40;
+        }
+    } else if(mode==40) {
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = 30;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                if(count>30000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==1) {
+                    mode=41;
+                    count=0;
+                    cross=0;
                 }
-            }
-        } else if(mode==31) {
-            motor[TIRE_FR].pwm = 15;
-            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 = 15;
-            motor[TIRE_BL].dir = FOR;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                mode=32;
-            }
-        } else if(mode==32) {
-            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(count>5000&&g[3]==0) {
-                count=0;
-                cross=0;
-                mode=40;
-            }
-        } else if(mode==40) {
-            switch(g[3]) {
-                case 98:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    if(count>30000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==1) {
-                        mode=41;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                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;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    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_FL].pwm += 20;
                     motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_BL].pwm += 20;
                     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;
-                    mtc2=true;
+                    mtc2=false;
                     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) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=42;
+        }
+    } else if(mode==42) {
+        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 = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = BACK;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=50;
+        }
+    } else if(mode==50) {
+        switch(g[1]) {
+            case 98:
+                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(count>100000) {
+                    cross++;
+                    count=0;
+                }
+                mtc=true;
+                if(cross==1) {
+                    mode=51;
+                    count=0;
                 }
-            }
-        } else if(mode==41) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=42;
-            }
-        } else if(mode==42) {
-            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 = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = BACK;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=50;
-            }
-        } else if(mode==50) {
-            switch(g[1]) {
-                case 98:
-                    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(count>100000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc=true;
-                    if(cross==1) {
-                        mode=51;
-                        count=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                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;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                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;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[1]!=98&&mtc==true) {
+            switch(g[0]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FR].pwm += 20;
                     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;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
                     break;
                 case 3:
-                    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;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
                     break;
                 case 2:
+                    motor[TIRE_FL].pwm += 20;
                     motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = FREE;
-                    mtc=true;
+                    mtc=false;
                     break;
             }
-            if(g[1]!=98&&mtc==true) {
-                switch(g[0]) {
-                    case 0:
-                        mtc=false;
-                        break;
-                    case 255:
-                        motor[TIRE_FR].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 253:
-                        motor[TIRE_FR].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 254:
-                        motor[TIRE_FR].pwm += 20;
-                        motor[TIRE_FL].pwm = 0;
-                        mtc=false;
-                        break;
-                    case 1:
-                        motor[TIRE_FL].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 3:
-                        motor[TIRE_FL].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 2:
-                        motor[TIRE_FL].pwm += 20;
-                        motor[TIRE_FR].pwm = 0;
-                        mtc=false;
-                        break;
+        }
+    } else if(mode==51) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            mode=52;
+        }
+    } else if(mode==52) {
+        motor[TIRE_FR].pwm = 15;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = FOR;
+        if(count>5000&&g[3]==0) {
+            count=0;
+            cross=0;
+            mode=60;
+        }
+    } else if(mode==60) {
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = 30;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                if(count>50000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=61;
+                    count=0;
+                    cross=0;
                 }
-            }
-        } else if(mode==51) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                mode=52;
-            }
-        } else if(mode==52) {
-            motor[TIRE_FR].pwm = 15;
-            motor[TIRE_FR].dir = BACK;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = FOR;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = FOR;
-            if(count>5000&&g[3]==0) {
-                count=0;
-                cross=0;
-                mode=60;
-            }
-        } else if(mode==60) {
-            switch(g[3]) {
-                case 98:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    if(count>50000) {
-                        cross++;
-                        count=0;
-                    }
-                    mtc2=true;
-                    if(cross==2) {
-                        mode=61;
-                        count=0;
-                        cross=0;
-                    }
-                    break;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                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 = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                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;
+                mtc2=true;
+                break;
+            case 254:
+                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;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                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;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    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 = FOR;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 253:
-                    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;
-                    mtc2=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 254:
-                    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_FL].pwm += 20;
                     motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc2=true;
+                    mtc2=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc2=true;
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_BL].pwm += 20;
                     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;
-                    mtc2=true;
+                    mtc2=false;
                     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==61) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=62;
+        }
+    } else if(mode==62) {
+        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 = BACK;
+        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].dir = BACK;
+        if(count>5000&&g[1]==0) {
+            count=0;
+            mode=70;
+        }
+    } else if(mode==70) {
+        switch(g[1]) {
+            case 99:
+                if(count>=100000) {
+                    mode=100;
                 }
-            }
-        } else if(mode==61) {
-            motor[TIRE_FR].pwm = 0;
-            motor[TIRE_FR].dir = FREE;
-            motor[TIRE_FL].pwm = 15;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 15;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 0;
-            motor[TIRE_BL].dir = FREE;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=62;
-            }
-        } else if(mode==62) {
-            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 = BACK;
-            motor[TIRE_BL].pwm = 15;
-            motor[TIRE_BL].dir = BACK;
-            if(count>5000&&g[1]==0) {
-                count=0;
-                mode=70;
-            }
-        } else if(mode==70) {
-            switch(g[1]) {
-                case 99:
-                    if(count>=100000) {
-                        mode=100;
-                    }
-                    break;
-                case 98:
-                    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;
-                    break;
+                break;
+            case 98:
+                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;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = 60;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 40;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 40;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 60;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 20;
+                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 = 20;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                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;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = 40;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 60;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = 60;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 40;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                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;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                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 = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[1]!=98&&mtc==true) {
+            switch(g[0]) {
                 case 0:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 60;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 60;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = 20;
-                    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 = 20;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FR].pwm += 20;
                     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;
-                    mtc=true;
+                    mtc=false;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = 60;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = 60;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = FOR;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
                     break;
                 case 3:
-                    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;
-                    mtc=true;
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
                     break;
                 case 2:
+                    motor[TIRE_FL].pwm += 20;
                     motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    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 = FREE;
-                    mtc=true;
+                    mtc=false;
                     break;
             }
-            if(g[1]!=98&&mtc==true) {
-                switch(g[0]) {
-                    case 0:
-                        mtc=false;
-                        break;
-                    case 255:
-                        motor[TIRE_FR].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 253:
-                        motor[TIRE_FR].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 254:
-                        motor[TIRE_FR].pwm += 20;
-                        motor[TIRE_FL].pwm = 0;
-                        mtc=false;
-                        break;
-                    case 1:
-                        motor[TIRE_FL].pwm += 5;
-                        mtc=false;
-                        break;
-                    case 3:
-                        motor[TIRE_FL].pwm += 10;
-                        mtc=false;
-                        break;
-                    case 2:
-                        motor[TIRE_FL].pwm += 20;
-                        motor[TIRE_FR].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 {
-            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;
         }
+    } 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 {
+        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
 
-#if USE_PROCESS_NUM>3
-static void Process3()
-{
-
-}
-#endif
-
 #if USE_PROCESS_NUM>4
 static void Process4()
 {