aa

Dependencies:   mbed

Revision:
52:4394a1debceb
Parent:
51:a0bdbddd08f7
Child:
53:694dbd7dfa0e
diff -r a0bdbddd08f7 -r 4394a1debceb System/Process/Process.cpp
--- a/System/Process/Process.cpp	Wed Nov 13 20:59:54 2019 +0000
+++ b/System/Process/Process.cpp	Wed Nov 13 21:21:44 2019 +0000
@@ -1945,688 +1945,1010 @@
 //Clearly 赤ゾーン
     LED::LED::TapeledMode(3);
     countss++;
-    if(mode==1) {//スタートゾーンから白線検知
-        fast=40;
-        normal=30;
-        slow=20;
-        motor[TIRE_FR].pwm = normal+20;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = normal+20;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = normal+20;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = normal+20;
-        motor[TIRE_BL].dir = FOR;
-        if(g[2]==0&&countss>=100000) {
-            countss=100000;
-            cross=0;
-            mode=3;
-        }
-    } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 255;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 30;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 30;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 255;
-        motor[TIRE_BL].dir = BRAKE;
-        if(g[0]==0) {
-            mode=10;
-            countss=100000;
-        }
-    } else if(mode==10) { //縦ライントレース
-        switch(g[0]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = normal+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal+50;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                if(countss>80000) {
-                    cross++;
-                    countss=0;
-                }
-                if(cross==2) {
-                    fast = 10;
-                    normal = 10;
-                    slow = 5;
-                }
-
-                if(cross==3) {
-                    fast=40;
-                    normal=30;
-                    slow=20;
-                    mode=11;
-                    countss=0;
-                    cross=0;
+    switch(mode) {
+        case 1:
+            fast=40;
+            normal=30;
+            slow=20;
+            motor[TIRE_FR].pwm = normal+20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = normal+20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = normal+20;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = normal+20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]==0&&countss>=100000) {
+                countss=100000;
+                cross=0;
+                mode=3;
+            }
+            break;
+        case 3:
+            motor[TIRE_FR].pwm = 255;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 30;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 30;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 255;
+            motor[TIRE_BL].dir = BRAKE;
+            if(g[0]==0) {
+                mode=10;
+                countss=100000;
+            }
+            break;
+        case 10:
+            switch(g[0]) {
+                case 98:
+                    motor[TIRE_FR].pwm = normal+50;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = normal+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = normal+50;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = normal+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
+                    if(countss>80000) {
+                        cross++;
+                        countss=0;
+                    }
+                    if(cross==2) {
+                        fast = 10;
+                        normal = 10;
+                        slow = 5;
+                    }
 
-                }
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = fast+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast+50;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = fast+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = slow+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow+50;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = slow+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = slow+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = normal+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = normal+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = slow+50;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast+50;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = slow+50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow+50;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = normal+50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal+50;
-                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 += 3;
-                    mtc=false;
-                    break;
-                case 253:
-                    motor[TIRE_BR].pwm += 6;
-                    mtc=false;
-                    break;
-                case 254:
-                    motor[TIRE_BR].pwm += 9;
-                    motor[TIRE_BL].pwm = 0;
-                    mtc=false;
-                    break;
-                case 1:
-                    motor[TIRE_BL].pwm += 3;
-                    mtc=false;
-                    break;
-                case 3:
-                    motor[TIRE_BL].pwm += 6;
-                    mtc=false;
-                    break;
-                case 2:
-                    motor[TIRE_BL].pwm += 9;
-                    motor[TIRE_BR].pwm = 0;
-                    mtc=false;
-                    break;
-            }
-        }
-    } else if(mode==11) {
-        motor[TIRE_FR].pwm = 25;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = normal;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = normal;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 25;
-        motor[TIRE_BL].dir = BACK;
-        if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
-            countss=0;
-            cross=0;
-            mode=20;
-        }
-    } else if(mode==20) {
-        solenoid.solenoid4= SOLENOID_ON;
-        switch(g[2]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal+20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = normal+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal+20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal+20;
-                motor[TIRE_BL].dir = BACK;
-                if(countss>30000) {
-                    cross++;
-                    countss=0;
-                }
-                mtc2=true;
-
-                if(YELLOW) {
-                    if(cross==1) {
-                        fast=20;
-                        normal=15;
-                        slow=15;
-                    } else if(cross==2) {
-                        mode=21;
-                        countss=0;
-                        cross=0;
-                    }
-                } else {
                     if(cross==3) {
-                        mode=21;
+                        fast=40;
+                        normal=30;
+                        slow=20;
+                        mode=11;
                         countss=0;
                         cross=0;
+
                     }
-                }
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = fast+20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast+20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast+20;
-                motor[TIRE_BL].dir = BACK;
-                mtc2=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = normal+20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast+20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal+20;
-                motor[TIRE_BL].dir = BACK;
-                mtc2=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow+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 = normal+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal+20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc2=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = fast+20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = slow+20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow+20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast+20;
-                motor[TIRE_BL].dir = BACK;
-                mtc2=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = slow+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 = slow+20;
-                motor[TIRE_BL].dir = BACK;
-                mtc2=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = normal+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 = normal+20;
-                motor[TIRE_BL].dir = BACK;
-                mtc2=true;
-                break;
-        }
-        if(g[2]!=98&&mtc2==true) {
-            switch(g[3]) {
+                    break;
                 case 0:
-                    mtc2=false;
+                    motor[TIRE_FR].pwm = fast+50;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = fast+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = fast+50;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = fast+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
                     break;
                 case 255:
-                    motor[TIRE_BL].pwm += 5;
-                    mtc2=false;
+                    motor[TIRE_FR].pwm = fast+50;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = slow+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = slow+50;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = fast+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
                     break;
                 case 253:
-                    motor[TIRE_BL].pwm += 10;
-                    mtc2=false;
+                    motor[TIRE_FR].pwm = slow+50;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = 0;
+                    motor[TIRE_FL].dir = FREE;
+                    motor[TIRE_BR].pwm = 0;
+                    motor[TIRE_BR].dir = FREE;
+                    motor[TIRE_BL].pwm = slow+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
                     break;
                 case 254:
-                    motor[TIRE_BL].pwm += 15;
+                    motor[TIRE_FR].pwm = normal+50;
+                    motor[TIRE_FR].dir = BACK;
                     motor[TIRE_FL].pwm = 0;
-                    mtc2=false;
+                    motor[TIRE_FL].dir = FREE;
+                    motor[TIRE_BR].pwm = 0;
+                    motor[TIRE_BR].dir = FREE;
+                    motor[TIRE_BL].pwm = normal+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
                     break;
                 case 1:
-                    motor[TIRE_FL].pwm += 5;
-                    mtc2=false;
+                    motor[TIRE_FR].pwm = slow+50;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = fast+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = fast+50;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = slow+50;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc=true;
                     break;
                 case 3:
-                    motor[TIRE_FL].pwm += 10;
-                    mtc2=false;
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = slow+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = slow+50;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = 0;
+                    motor[TIRE_BL].dir = FREE;
+                    mtc=true;
                     break;
                 case 2:
-                    motor[TIRE_FL].pwm += 15;
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = normal+50;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = normal+50;
+                    motor[TIRE_BR].dir = BACK;
                     motor[TIRE_BL].pwm = 0;
-                    mtc2=false;
+                    motor[TIRE_BL].dir = FREE;
+                    mtc=true;
                     break;
             }
-        }
-    }
-    if(YELLOW) {
-        if(mode==21) {
-            motor[TIRE_FR].pwm = 20;
-            motor[TIRE_FR].dir = BACK;
-            motor[TIRE_FL].pwm = 20;
-            motor[TIRE_FL].dir = BACK;
-            motor[TIRE_BR].pwm = 20;
-            motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 20;
-            motor[TIRE_BL].dir = FOR;
-            if(g[3]==98&&countss>=10000) {
-                mode=22;
+            if(g[0]!=98&&mtc==true) {
+                switch(g[1]) {
+                    case 0:
+                        mtc=false;
+                        break;
+                    case 255:
+                        motor[TIRE_BR].pwm += 3;
+                        mtc=false;
+                        break;
+                    case 253:
+                        motor[TIRE_BR].pwm += 6;
+                        mtc=false;
+                        break;
+                    case 254:
+                        motor[TIRE_BR].pwm += 9;
+                        motor[TIRE_BL].pwm = 0;
+                        mtc=false;
+                        break;
+                    case 1:
+                        motor[TIRE_BL].pwm += 3;
+                        mtc=false;
+                        break;
+                    case 3:
+                        motor[TIRE_BL].pwm += 6;
+                        mtc=false;
+                        break;
+                    case 2:
+                        motor[TIRE_BL].pwm += 9;
+                        motor[TIRE_BR].pwm = 0;
+                        mtc=false;
+                        break;
+                }
+            }
+            break;
+        case 11:
+            motor[TIRE_FR].pwm = 25;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = normal;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = normal;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 25;
+            motor[TIRE_BL].dir = BACK;
+            if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
                 countss=0;
-                fast=60;
-                normal=40;
-                slow=20;
+                cross=0;
+                mode=20;
             }
-        } else if(mode==22) {
-            if(g[4]==3||g[4]==2) {
-                motor[TIRE_FR].pwm = 30;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 30;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 30;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 30;
-                motor[TIRE_BL].dir = BACK;
-            }
-            if(g[4]==253||g[4]==254) {
-                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;
-            }
+            break;
+        case 20:
+            solenoid.solenoid4= SOLENOID_ON;
             switch(g[2]) {
+                case 98:
+                    motor[TIRE_FR].pwm = normal+20;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = normal+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = normal+20;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = normal+20;
+                    motor[TIRE_BL].dir = BACK;
+                    if(countss>30000) {
+                        cross++;
+                        countss=0;
+                    }
+                    mtc2=true;
+
+                    if(YELLOW) {
+                        if(cross==1) {
+                            fast=20;
+                            normal=15;
+                            slow=15;
+                        } else if(cross==2) {
+                            mode=21;
+                            countss=0;
+                            cross=0;
+                        }
+                    } else {
+                        if(cross==3) {
+                            mode=21;
+                            countss=0;
+                            cross=0;
+                        }
+                    }
+                    break;
                 case 0:
-                    motor[TIRE_FR].pwm = 20;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 20;
-                    motor[TIRE_BR].dir = FREE;
+                    motor[TIRE_FR].pwm = fast+20;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = fast+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = fast+20;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = fast+20;
+                    motor[TIRE_BL].dir = BACK;
+                    mtc2=true;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 20;
-                    motor[TIRE_BR].dir = FREE;
+                    motor[TIRE_FR].pwm = normal+20;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = fast+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = fast+20;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = normal+20;
+                    motor[TIRE_BL].dir = BACK;
+                    mtc2=true;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 20;
-                    motor[TIRE_BR].dir = FREE;
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = slow+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = slow+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 = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 30;
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = normal+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = normal+20;
                     motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = 0;
+                    motor[TIRE_BL].dir = FREE;
+                    mtc2=true;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = 20;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_FR].pwm = fast+20;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = slow+20;
+                    motor[TIRE_FL].dir = FOR;
+                    motor[TIRE_BR].pwm = slow+20;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = fast+20;
+                    motor[TIRE_BL].dir = BACK;
+                    mtc2=true;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = 20;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_FR].pwm = slow+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 = slow+20;
+                    motor[TIRE_BL].dir = BACK;
+                    mtc2=true;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = 30;
+                    motor[TIRE_FR].pwm = normal+20;
                     motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = FOR;
-                    break;
-            }
-            switch(g[3]) {
-                case 0:
-                    motor[TIRE_FL].pwm = 20;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BL].pwm = 20;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 255:
-                    motor[TIRE_FL].pwm = 20;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BL].pwm = 30;
-                    motor[TIRE_BL].dir = BACK;
-                case 253:
-                    motor[TIRE_FL].pwm = 20;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 254:
-                    motor[TIRE_FL].pwm = 30;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BL].pwm = 30;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 1:
-                    motor[TIRE_FL].pwm = 30;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BL].pwm = 20;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 3:
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BL].pwm = 20;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 2:
-                    motor[TIRE_FL].pwm = 30;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BL].pwm = 30;
-                    motor[TIRE_BL].dir = FOR;
-                    break;
-            }
-            if(g[3]==0&&g[2]==0) {
-                mode=23;
-                countss=0;
-            }
-        } else if(mode==23) {
-            switch(g[4]) {
-                case 98:
-                    motor[TIRE_FR].pwm = fast-5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast-5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast-5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast-5;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 99:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
                     motor[TIRE_FL].pwm = 0;
                     motor[TIRE_FL].dir = FREE;
                     motor[TIRE_BR].pwm = 0;
                     motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 0:
-                    motor[TIRE_FR].pwm = fast-5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast-5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast-5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast-5;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_FL].pwm = 0;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BR].pwm = 0;
-                    motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 253:
-                    motor[TIRE_FR].pwm = slow+5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast-5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast-5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = slow+5;
+                    motor[TIRE_BL].pwm = normal+20;
                     motor[TIRE_BL].dir = BACK;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm = normal+5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast-5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast-5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = normal+5;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_FL].pwm = 0;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BR].pwm = 0;
-                    motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 3:
-                    motor[TIRE_FR].pwm = fast-5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = slow+5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = slow+5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast-5;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 2:
-                    motor[TIRE_FR].pwm = fast-5;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = normal+5;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = normal+5;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast-5;
-                    motor[TIRE_BL].dir = BACK;
+                    mtc2=true;
                     break;
             }
-            if(g[1]==98) {
-                fast=20;
-                normal=15;
-                slow=10;
+            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 += 15;
+                        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 += 15;
+                        motor[TIRE_BL].pwm = 0;
+                        mtc2=false;
+                        break;
+                }
             }
-            if(g[3]==254&&countss>=100000) {
-                mode=24;
-                solenoid.solenoid4 = SOLENOID_OFF;
-                countss=0;
-                fast=60;
-                normal=40;
-                slow=20;
-            }
-        } else if(mode==24) {
-            if(g[4]==3||g[4]==2) {
+            break;
+        case 21:
+            if(YELLOW) {
+                motor[TIRE_FR].pwm = 20;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 20;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 20;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 20;
+                motor[TIRE_BL].dir = FOR;
+                if(g[3]==98&&countss>=10000) {
+                    mode=22;
+                    countss=0;
+                    fast=60;
+                    normal=40;
+                    slow=20;
+                }
+            } else {
                 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_FL].pwm = 255;
+                motor[TIRE_FL].dir = BRAKE;
+                motor[TIRE_BR].pwm = 255;
+                motor[TIRE_BR].dir = BRAKE;
                 motor[TIRE_BL].pwm = 30;
                 motor[TIRE_BL].dir = BACK;
-            }
-            if(g[4]==253||g[4]==254) {
-                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(g[1]==0) {
+                    mode=22;
+                    fast=60;
+                    normal=40;
+                    slow=25;
+                }
             }
-            switch(g[2]) {
-                case 0:
-                    motor[TIRE_FR].pwm = 25;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 25;
-                    motor[TIRE_BR].dir = FREE;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 25;
-                    motor[TIRE_BR].dir = FREE;
-                    break;
-                case 253:
-                    motor[TIRE_FR].pwm = 40;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 25;
-                    motor[TIRE_BR].dir = FREE;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = BACK;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm = 25;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = FOR;
-                    break;
-                case 3:
-                    motor[TIRE_FR].pwm = 25;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
-                    break;
-                case 2:
+            break;
+        case 22:
+            if(YELLOW) {
+                if(g[4]==3||g[4]==2) {
                     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 = FOR;
-                    break;
-            }
-            switch(g[3]) {
-                case 0:
-                    motor[TIRE_FL].pwm = 25;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BL].pwm = 25;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 255:
-                    motor[TIRE_FL].pwm = 25;
-                    motor[TIRE_FL].dir = FREE;
+                    motor[TIRE_BR].dir = BACK;
                     motor[TIRE_BL].pwm = 30;
                     motor[TIRE_BL].dir = BACK;
-                case 253:
-                    motor[TIRE_FL].pwm = 25;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BL].pwm = 40;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 254:
+                }
+                if(g[4]==253||g[4]==254) {
+                    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 = BACK;
-                    break;
-                case 1:
+                    motor[TIRE_BL].dir = FOR;
+                }
+                switch(g[2]) {
+                    case 0:
+                        motor[TIRE_FR].pwm = 20;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 20;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 20;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = 40;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 20;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = BACK;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = 20;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 20;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 40;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                }
+                switch(g[3]) {
+                    case 0:
+                        motor[TIRE_FL].pwm = 20;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 20;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 255:
+                        motor[TIRE_FL].pwm = 20;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = BACK;
+                    case 253:
+                        motor[TIRE_FL].pwm = 20;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 40;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 254:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 1:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 20;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 3:
+                        motor[TIRE_FL].pwm = 40;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 20;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 2:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                }
+                if(g[3]==0&&g[2]==0) {
+                    mode=23;
+                    countss=0;
+                }
+            } else {
+                if(g[1]==98) {
+                    slow=25;
+                    normal=30;
+                    fast=40;
+                }
+                if(g[3]==0) {
+                    slow=25;
+                    normal=40;
+                    fast=60;
+                    mode=23;
+                }
+                switch(g[1]) {
+                    case 98:
+                        solenoid.solenoid4 = SOLENOID_OFF;
+                        fast=40;
+                        normal=30;
+                        slow=25;
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 0:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                }
+                if(g[1]!=98&&mtc==true) {
+                    switch(g[0]) {
+                        case 0:
+                            mtc=false;
+                            break;
+                        case 255:
+                            motor[TIRE_FL].pwm += 3;
+                            mtc=false;
+                            break;
+                        case 253:
+                            motor[TIRE_FL].pwm += 6;
+                            mtc=false;
+                            break;
+                        case 254:
+                            motor[TIRE_FL].pwm += 9;
+                            motor[TIRE_FR].pwm = 0;
+                            mtc=false;
+                            break;
+                        case 1:
+                            motor[TIRE_FR].pwm += 3;
+                            mtc=false;
+                            break;
+                        case 3:
+                            motor[TIRE_FR].pwm += 6;
+                            mtc=false;
+                            break;
+                        case 2:
+                            motor[TIRE_FR].pwm += 9;
+                            motor[TIRE_FL].pwm = 0;
+                            mtc=false;
+                            break;
+                    }
+                }
+            }
+            break;
+        case 23:
+            if(YELLOW) {
+                switch(g[4]) {
+                    case 98:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 99:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 0:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = slow+5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = slow+5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = normal+5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = normal+5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = slow+5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = slow+5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = normal+5;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = normal+5;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                }
+                if(g[1]==98) {
+                    fast=20;
+                    normal=15;
+                    slow=10;
+                }
+                if(g[3]==254&&countss>=100000) {
+                    mode=24;
+                    solenoid.solenoid4 = SOLENOID_OFF;
+                    countss=0;
+                    fast=60;
+                    normal=40;
+                    slow=20;
+                }
+            } else {
+                if(UPLS||DOLS) {
+                    if(UPLS||DOLS) {
+                        motor[MOTOR_0].pwm = 200;
+                        motor[MOTOR_0].dir = BRAKE;
+                        fast=60;
+                        normal=40;
+                        slow=25;
+                        countss=0;
+                        mode=24;
+                    }
+                } else {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BACK;
+                }
+                motor[TIRE_FR].pwm = 100;
+                motor[TIRE_FR].dir = BRAKE;
+                motor[TIRE_FL].pwm = 100;
+                motor[TIRE_FL].dir = BRAKE;
+                motor[TIRE_BR].pwm = 100;
+                motor[TIRE_BR].dir = BRAKE;
+                motor[TIRE_BL].pwm = 100;
+                motor[TIRE_BL].dir = BRAKE;
+            }
+            break;
+        case 24:
+            if(YELLOW) {
+                if(g[4]==3||g[4]==2) {
+                    motor[TIRE_FR].pwm = 30;
+                    motor[TIRE_FR].dir = FOR;
                     motor[TIRE_FL].pwm = 30;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BL].pwm = 25;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 3:
-                    motor[TIRE_FL].pwm = 40;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BL].pwm = 25;
-                    motor[TIRE_BL].dir = FREE;
-                    break;
-                case 2:
+                    motor[TIRE_BR].pwm = 30;
+                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_BL].pwm = 30;
+                    motor[TIRE_BL].dir = BACK;
+                }
+                if(g[4]==253||g[4]==254) {
+                    motor[TIRE_FR].pwm = 30;
+                    motor[TIRE_FR].dir = BACK;
                     motor[TIRE_FL].pwm = 30;
-                    motor[TIRE_FL].dir = FOR;
+                    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;
-                    break;
+                }
+                switch(g[2]) {
+                    case 0:
+                        motor[TIRE_FR].pwm = 25;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 25;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 25;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = 40;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 25;
+                        motor[TIRE_BR].dir = FREE;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = BACK;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = 25;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 25;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_BR].pwm = 40;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 30;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_BR].pwm = 30;
+                        motor[TIRE_BR].dir = FOR;
+                        break;
+                }
+                switch(g[3]) {
+                    case 0:
+                        motor[TIRE_FL].pwm = 25;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 25;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 255:
+                        motor[TIRE_FL].pwm = 25;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = BACK;
+                    case 253:
+                        motor[TIRE_FL].pwm = 25;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BL].pwm = 40;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 254:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 1:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 25;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 3:
+                        motor[TIRE_FL].pwm = 40;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 25;
+                        motor[TIRE_BL].dir = FREE;
+                        break;
+                    case 2:
+                        motor[TIRE_FL].pwm = 30;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BL].pwm = 30;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                }
+                if(g[3]==0&&g[2]==0) {
+                    mode=25;
+                }
+            } else {
+                if(g[1]==98&&countss>=40000) {
+                    mode=31;
+                    countss=0;
+                }
+                switch(g[1]) {
+                    case 98:
+                        fast=40;
+                        normal=30;
+                        slow=25;
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = BACK;
+                        break;
+                    case 0:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = FOR;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = BACK;
+                        mtc=true;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = BACK;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = FOR;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                }
+                if(g[1]!=98&&mtc==true) {
+                    switch(g[0]) {
+                        case 0:
+                            mtc=false;
+                            break;
+                        case 255:
+                            motor[TIRE_FL].pwm += 3;
+                            mtc=false;
+                            break;
+                        case 253:
+                            motor[TIRE_FL].pwm += 6;
+                            mtc=false;
+                            break;
+                        case 254:
+                            motor[TIRE_FL].pwm += 9;
+                            motor[TIRE_FR].pwm = 0;
+                            mtc=false;
+                            break;
+                        case 1:
+                            motor[TIRE_FR].pwm += 3;
+                            mtc=false;
+                            break;
+                        case 3:
+                            motor[TIRE_FR].pwm += 6;
+                            mtc=false;
+                            break;
+                        case 2:
+                            motor[TIRE_FR].pwm += 9;
+                            motor[TIRE_FL].pwm = 0;
+                            mtc=false;
+                            break;
+                    }
+                }
             }
-            if(g[3]==0&&g[2]==0) {
-                mode=25;
-            }
-        } else if(mode==25) {
+            break;
+        case 25:
             motor[TIRE_FR].pwm = 100;
             motor[TIRE_FR].dir = BRAKE;
             motor[TIRE_FL].pwm = 100;
@@ -2643,7 +2965,8 @@
                 motor[MOTOR_0].pwm = 100;
                 motor[MOTOR_0].dir = BACK;
             }
-        } else if(mode==26) {
+            break;
+        case 26:
             switch(g[4]) {
                 case 98:
                     motor[TIRE_FR].pwm = 60;
@@ -2739,58 +3062,227 @@
             if(g[1]==98) {
                 mode=31;
             }
-        }
-    } else {
-        if(mode==21) {
-            motor[TIRE_FR].pwm = 30;
+            break;
+        case 31:
+            motor[TIRE_FR].pwm = 255;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 45;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 45;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 255;
+            motor[TIRE_BL].dir = BRAKE;
+            if(g[2]==0) {
+                cross=0;
+                countss=0;
+                mode=40;
+            }
+            break;
+        case 40:
+            switch(g[3]) {
+                case 98:
+                    motor[TIRE_FR].pwm = normal+15;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = normal+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = normal+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = normal+15;
+                    motor[TIRE_BL].dir = FOR;
+                    if(countss>30000) {
+                        cross++;
+                        countss=0;
+                    }
+                    mtc2=true;
+                    break;
+                case 0:
+                    motor[TIRE_FR].pwm = fast+15;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = fast+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = fast+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = fast+15;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc2=true;
+                    break;
+                case 255:
+                    motor[TIRE_FR].pwm = slow+15;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = fast+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = fast+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = slow+15;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc2=true;
+                    break;
+                case 253:
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = slow+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = slow+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = 0;
+                    motor[TIRE_BL].dir = FREE;
+                    mtc2=true;
+                    break;
+                case 254:
+                    motor[TIRE_FR].pwm = 0;
+                    motor[TIRE_FR].dir = FREE;
+                    motor[TIRE_FL].pwm = normal+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = normal+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = 0;
+                    motor[TIRE_BL].dir = FREE;
+                    mtc2=true;
+                    break;
+                case 1:
+                    motor[TIRE_FR].pwm = fast+15;
+                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FL].pwm = slow+15;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = slow+15;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = fast+15;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc2=true;
+                    break;
+                case 3:
+                    motor[TIRE_FR].pwm = slow+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 = slow+15;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc2=true;
+                    break;
+                case 2:
+                    motor[TIRE_FR].pwm = normal+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 = normal+15;
+                    motor[TIRE_BL].dir = FOR;
+                    mtc2=true;
+                    break;
+            }
+            if(g[2]!=98&&mtc2==true) {
+                switch(g[2]) {
+                    case 0:
+                        mtc2=false;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm += 3;
+                        mtc2=false;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm += 6;
+                        mtc2=false;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm += 9;
+                        motor[TIRE_BR].pwm = 0;
+                        mtc2=false;
+                        break;
+                    case 1:
+                        motor[TIRE_BR].pwm += 3;
+                        mtc2=false;
+                        break;
+                    case 3:
+                        motor[TIRE_BR].pwm += 6;
+                        mtc2=false;
+                        break;
+                    case 2:
+                        motor[TIRE_BR].pwm += 9;
+                        motor[TIRE_FR].pwm = 0;
+                        mtc2=false;
+                        break;
+                }
+            }
+
+            if(YELLOW) {
+                if(cross==1) {
+                    mode=41;
+                    countss=0;
+                    cross=0;
+                }
+            } else {
+                if(cross==3) {
+                    mode=41;
+                    countss=0;
+                    cross=0;
+                }
+            }
+            if(LIF||LIB) {
+                mode=42;
+            }
+            break;
+        case 41:
+            if(LIF||LIB) {
+                mode=42;
+            }
+            motor[TIRE_FR].pwm = 255;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 40;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 40;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 255;
+            motor[TIRE_BL].dir = BRAKE;
+            if(g[1]==0) {
+                countss=0;
+                mode=70;
+                normal=80;
+                fast=100;
+                slow=60;
+            }
+            break;
+        case 42:
+            motor[TIRE_FR].pwm = 40;
             motor[TIRE_FR].dir = FOR;
-            motor[TIRE_FL].pwm = 255;
-            motor[TIRE_FL].dir = BRAKE;
-            motor[TIRE_BR].pwm = 255;
-            motor[TIRE_BR].dir = BRAKE;
-            motor[TIRE_BL].pwm = 30;
+            motor[TIRE_FL].pwm = 0;
+            motor[TIRE_FL].dir = FREE;
+            motor[TIRE_BR].pwm = 0;
+            motor[TIRE_BR].dir = FREE;
+            motor[TIRE_BL].pwm = 40;
             motor[TIRE_BL].dir = BACK;
             if(g[1]==0) {
-                mode=22;
-                fast=60;
-                normal=40;
-                slow=25;
-            }
-        } else if(mode==22) {
-            if(g[1]==98) {
-                slow=25;
-                normal=30;
-                fast=40;
+                countss=0;
+                mode=70;
             }
-            if(g[3]==0) {
-                slow=25;
-                normal=40;
-                fast=60;
-                mode=23;
-            }
+            break;
+        case 70:
             switch(g[1]) {
+                case 99:
+                    if(countss>=100000) {
+                        mode=100;
+                    }
+                    break;
                 case 98:
-                    solenoid.solenoid4 = SOLENOID_OFF;
-                    fast=40;
-                    normal=30;
-                    slow=25;
                     motor[TIRE_FR].pwm = normal;
                     motor[TIRE_FR].dir = FOR;
                     motor[TIRE_FL].pwm = normal;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = normal;
+                    motor[TIRE_BR].pwm = normal+5;
                     motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = normal;
                     motor[TIRE_BL].dir = BACK;
                     break;
                 case 0:
-                    motor[TIRE_FR].pwm = fast;
+                    motor[TIRE_FR].pwm = 100;
                     motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast;
+                    motor[TIRE_FL].pwm = 100;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast;
+                    motor[TIRE_BR].pwm = 105;
                     motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast;
+                    motor[TIRE_BL].pwm = 100;
                     motor[TIRE_BL].dir = BACK;
                     mtc=true;
                     break;
@@ -2799,7 +3291,7 @@
                     motor[TIRE_FR].dir = FOR;
                     motor[TIRE_FL].pwm = slow;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = slow;
+                    motor[TIRE_BR].pwm = slow+5;
                     motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = fast;
                     motor[TIRE_BL].dir = BACK;
@@ -2832,7 +3324,7 @@
                     motor[TIRE_FR].dir = FOR;
                     motor[TIRE_FL].pwm = fast;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast;
+                    motor[TIRE_BR].pwm = fast+5;
                     motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = slow;
                     motor[TIRE_BL].dir = BACK;
@@ -2843,7 +3335,7 @@
                     motor[TIRE_FR].dir = FREE;
                     motor[TIRE_FL].pwm = slow;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = slow;
+                    motor[TIRE_BR].pwm = slow+5;
                     motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = 0;
                     motor[TIRE_BL].dir = FREE;
@@ -2854,160 +3346,7 @@
                     motor[TIRE_FR].dir = FREE;
                     motor[TIRE_FL].pwm = normal;
                     motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = normal;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc=true;
-                    break;
-            }
-            if(g[1]!=98&&mtc==true) {
-                switch(g[0]) {
-                    case 0:
-                        mtc=false;
-                        break;
-                    case 255:
-                        motor[TIRE_FL].pwm += 3;
-                        mtc=false;
-                        break;
-                    case 253:
-                        motor[TIRE_FL].pwm += 6;
-                        mtc=false;
-                        break;
-                    case 254:
-                        motor[TIRE_FL].pwm += 9;
-                        motor[TIRE_FR].pwm = 0;
-                        mtc=false;
-                        break;
-                    case 1:
-                        motor[TIRE_FR].pwm += 3;
-                        mtc=false;
-                        break;
-                    case 3:
-                        motor[TIRE_FR].pwm += 6;
-                        mtc=false;
-                        break;
-                    case 2:
-                        motor[TIRE_FR].pwm += 9;
-                        motor[TIRE_FL].pwm = 0;
-                        mtc=false;
-                        break;
-                }
-            }
-        } else if(mode==23) {
-            if(UPLS||DOLS) {
-                if(UPLS||DOLS) {
-                    motor[MOTOR_0].pwm = 200;
-                    motor[MOTOR_0].dir = BRAKE;
-                    fast=60;
-                    normal=40;
-                    slow=25;
-                    countss=0;
-                    mode=24;
-                }
-            } else {
-                motor[MOTOR_0].pwm = 100;
-                motor[MOTOR_0].dir = BACK;
-            }
-            motor[TIRE_FR].pwm = 100;
-            motor[TIRE_FR].dir = BRAKE;
-            motor[TIRE_FL].pwm = 100;
-            motor[TIRE_FL].dir = BRAKE;
-            motor[TIRE_BR].pwm = 100;
-            motor[TIRE_BR].dir = BRAKE;
-            motor[TIRE_BL].pwm = 100;
-            motor[TIRE_BL].dir = BRAKE;
-        } else if(mode==24) {
-            if(g[1]==98&&countss>=40000) {
-                mode=31;
-                countss=0;
-            }
-            switch(g[1]) {
-                case 98:
-                    fast=40;
-                    normal=30;
-                    slow=25;
-                    motor[TIRE_FR].pwm = normal;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = normal;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = normal;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = normal;
-                    motor[TIRE_BL].dir = BACK;
-                    break;
-                case 0:
-                    motor[TIRE_FR].pwm = fast;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm = fast;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = slow;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = slow;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = fast;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
-                    break;
-                case 253:
-                    motor[TIRE_FR].pwm = slow;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 0;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BR].pwm = 0;
-                    motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = slow;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm = normal;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = 0;
-                    motor[TIRE_FL].dir = FREE;
-                    motor[TIRE_BR].pwm = 0;
-                    motor[TIRE_BR].dir = FREE;
-                    motor[TIRE_BL].pwm = normal;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm = slow;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_FL].pwm = fast;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = fast;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = slow;
-                    motor[TIRE_BL].dir = BACK;
-                    mtc=true;
-                    break;
-                case 3:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_FL].pwm = slow;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = slow;
-                    motor[TIRE_BR].dir = FOR;
-                    motor[TIRE_BL].pwm = 0;
-                    motor[TIRE_BL].dir = FREE;
-                    mtc=true;
-                    break;
-                case 2:
-                    motor[TIRE_FR].pwm = 0;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_FL].pwm = normal;
-                    motor[TIRE_FL].dir = BACK;
-                    motor[TIRE_BR].pwm = normal;
+                    motor[TIRE_BR].pwm = normal+5;
                     motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = 0;
                     motor[TIRE_BL].dir = FREE;
@@ -3047,367 +3386,27 @@
                         break;
                 }
             }
-        }
-    }
-
-    /*
-    if(mode==31) {
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = FREE;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = FREE;
-        if(countss>=5000) {
-            countss=0;
-            mode=32;
-        }
-    } else if(mode==32) {
-        motor[TIRE_FR].pwm = 10;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 10;
-        motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 10;
-        motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 10;
-        motor[TIRE_BL].dir = BRAKE;
-        if(countss>=40000) {
-            mode=33;
-            countss=0;
-        }
-    }
-    */
-    if(mode==31) {
-        motor[TIRE_FR].pwm = 255;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 45;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 45;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 255;
-        motor[TIRE_BL].dir = BRAKE;
-        if(g[2]==0) {
-            cross=0;
-            countss=0;
-            mode=40;
-        }
-    } else if(mode==40) {
-        switch(g[3]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal+15;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = normal+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = normal+15;
-                motor[TIRE_BL].dir = FOR;
-                if(countss>30000) {
-                    cross++;
-                    countss=0;
-                }
-                mtc2=true;
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = fast+15;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast+15;
-                motor[TIRE_BL].dir = FOR;
-                mtc2=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = slow+15;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = slow+15;
-                motor[TIRE_BL].dir = FOR;
-                mtc2=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc2=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = normal+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc2=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = fast+15;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = slow+15;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+15;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast+15;
-                motor[TIRE_BL].dir = FOR;
-                mtc2=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = slow+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 = slow+15;
-                motor[TIRE_BL].dir = FOR;
-                mtc2=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = normal+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 = normal+15;
-                motor[TIRE_BL].dir = FOR;
-                mtc2=true;
-                break;
-        }
-        if(g[2]!=98&&mtc2==true) {
-            switch(g[2]) {
-                case 0:
-                    mtc2=false;
-                    break;
-                case 255:
-                    motor[TIRE_FR].pwm += 3;
-                    mtc2=false;
-                    break;
-                case 253:
-                    motor[TIRE_FR].pwm += 6;
-                    mtc2=false;
-                    break;
-                case 254:
-                    motor[TIRE_FR].pwm += 9;
-                    motor[TIRE_BR].pwm = 0;
-                    mtc2=false;
-                    break;
-                case 1:
-                    motor[TIRE_BR].pwm += 3;
-                    mtc2=false;
-                    break;
-                case 3:
-                    motor[TIRE_BR].pwm += 6;
-                    mtc2=false;
-                    break;
-                case 2:
-                    motor[TIRE_BR].pwm += 9;
-                    motor[TIRE_FR].pwm = 0;
-                    mtc2=false;
-                    break;
-            }
-        }
-
-        if(YELLOW) {
-            if(cross==1) {
-                mode=41;
-                countss=0;
-                cross=0;
-            }
-        } else {
-            if(cross==3) {
-                mode=41;
-                countss=0;
-                cross=0;
-            }
-        }
-        if(LIF||LIB) {
-            mode=42;
-        }
-
-    } else if(mode==41) {
-        if(LIF||LIB) {
-            mode=42;
-        }
-        motor[TIRE_FR].pwm = 255;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 40;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 40;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 255;
-        motor[TIRE_BL].dir = BRAKE;
-        if(g[1]==0) {
-            countss=0;
-            mode=70;
-            normal=80;
-            fast=100;
-            slow=60;
-        }
-    } else if(mode==42) {
-        motor[TIRE_FR].pwm = 40;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = FREE;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = 40;
-        motor[TIRE_BL].dir = BACK;
-        if(g[1]==0) {
-            countss=0;
-            mode=70;
-        }
-    } else if(mode==70) {
-        switch(g[1]) {
-            case 99:
-                if(countss>=100000) {
-                    mode=100;
-                }
-                break;
-            case 98:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+5;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = BACK;
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = 100;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 100;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 105;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 100;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = fast;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+5;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast+5;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+5;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+5;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-        }
-        if(g[1]!=98&&mtc==true) {
-            switch(g[0]) {
-                case 0:
-                    mtc=false;
-                    break;
-                case 255:
-                    motor[TIRE_FL].pwm += 3;
-                    mtc=false;
-                    break;
-                case 253:
-                    motor[TIRE_FL].pwm += 6;
-                    mtc=false;
-                    break;
-                case 254:
-                    motor[TIRE_FL].pwm += 9;
-                    motor[TIRE_FR].pwm = 0;
-                    mtc=false;
-                    break;
-                case 1:
-                    motor[TIRE_FR].pwm += 3;
-                    mtc=false;
-                    break;
-                case 3:
-                    motor[TIRE_FR].pwm += 6;
-                    mtc=false;
-                    break;
-                case 2:
-                    motor[TIRE_FR].pwm += 9;
-                    motor[TIRE_FL].pwm = 0;
-                    mtc=false;
-                    break;
-            }
-        }
-    } else if(mode==100) {
-        motor[TIRE_FR].pwm = 50;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 55;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 60;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 50;
-        motor[TIRE_BL].dir = BACK;
+            break;
+        case 100:
+            motor[TIRE_FR].pwm = 50;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 55;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 60;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 50;
+            motor[TIRE_BL].dir = BACK;
+            break;
+        default:
+            motor[TIRE_FR].pwm = 50;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 50;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 50;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 50;
+            motor[TIRE_BL].dir = BRAKE;
+            break;
     }
 
 }