aa

Dependencies:   mbed

Revision:
51:a0bdbddd08f7
Parent:
50:87f5fa1bc460
Child:
52:4394a1debceb
diff -r 87f5fa1bc460 -r a0bdbddd08f7 System/Process/Process.cpp
--- a/System/Process/Process.cpp	Wed Nov 13 10:00:45 2019 +0000
+++ b/System/Process/Process.cpp	Wed Nov 13 20:59:54 2019 +0000
@@ -473,612 +473,131 @@
     countss++;
     switch(mode) {
         case 1:
+            fast=40;
+            normal=30;
+            slow=20;
+            motor[TIRE_FR].pwm = normal+20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = normal+20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = normal+20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = normal+20;
+            motor[TIRE_BL].dir = BACK;
+            if(g[2]==0&&countss>=100000) {
+                countss=100000;
+                cross=0;
+                mode=3;
+            }
             break;
-        case 2:
+        case 3:
+            motor[TIRE_FR].pwm = 30;
+            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_BL].dir = BACK;
+            if(g[1]==0) {
+                mode=10;
+                countss=100000;
+            }
             break;
         case 10:
-            break;
-        case 11:
-            break;
-        case 20:
-            break;
-        case 21:
-            break;
-        case 22:
-            break;
-        case 23:
-            break;
-        case 24:
-            break;
-        case 25:
-            break;
-        case 26:
-            break;
-        case 31:
-            break;
-        case 40:
-            break;
-        case 41:
-            break;
-        case 42:
-            break;
-        case 70:
-            break;
-        case 100:
-            break;
-        default:
-             break;
-    }
-    if(mode==1) {//スタートゾーンから白線検知
-        fast=40;
-        normal=30;
-        slow=20;
-        motor[TIRE_FR].pwm = normal+20;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = normal+20;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = normal+20;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = normal+20;
-        motor[TIRE_BL].dir = BACK;
-        if(g[2]==0&&countss>=100000) {
-            countss=100000;
-            cross=0;
-            mode=3;
-        }
-    } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 30;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 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[1]==0) {
-            mode=10;
-            countss=100000;
-        }
-    } else if(mode==10) { //縦ライントレース
-        switch(g[1]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal+55;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = normal+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+50;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = normal+45;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                if(countss>80000) {
-                    cross++;
-                    countss=0;
-                }
-
-                if(cross==2) {
-                    fast = 10;
-                    normal = 10;
-                    slow = 5;
-                }
+            switch(g[1]) {
+                case 98:
+                    motor[TIRE_FR].pwm = normal+55;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = normal+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = normal+50;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = normal+45;
+                    motor[TIRE_BL].dir = BACK;
+                    mtc=true;
+                    if(countss>80000) {
+                        cross++;
+                        countss=0;
+                    }
 
-                if(cross==3) {
-                    fast=40;
-                    normal=30;
-                    slow=20;
-                    mode=11;
-                    countss=0;
-                    cross=0;
-                }
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = fast+55;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast+50;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast+45;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = fast+55;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = slow+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+50;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = fast+45;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = slow+55;
-                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+45;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = normal+55;
-                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+45;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = slow+55;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = fast+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = fast+50;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = slow+45;
-                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+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = slow+50;
-                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+50;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = normal+50;
-                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==11) {
-        motor[TIRE_FR].pwm = normal;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 50;
-        motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 50;
-        motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = normal;
-        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.solenoid2= SOLENOID_ON;
-        switch(g[2]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = BACK;
-                if(countss>20000) {
-                    cross++;
-                    countss=0;
-                }
-                mtc2=true;
+                    if(cross==2) {
+                        fast = 10;
+                        normal = 10;
+                        slow = 5;
+                    }
 
-                if(YELLOW) {
-                    if(cross==1) {
-                        fast=20;
-                        normal=15;
-                        slow=15;
-                    }
-                    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]) {
-                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;
-            }
-        }
-    } else if(mode==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 = 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=22;
-                fast=60;
-                normal=40;
-                slow=25;
-            }
-        }
-    } else if(mode==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 = 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;
-            }
-            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[0]==98) {
-                slow=25;
-                normal=30;
-                fast=40;
-            }
-            if(g[3]==0) {
-                slow=25;
-                normal=40;
-                fast=40;
-                mode=23;
-            }
-            switch(g[0]) {
-                case 98:
-                    solenoid.solenoid2 = SOLENOID_OFF;
-                    fast=40;
-                    normal=30;
-                    slow=25;
-                    motor[TIRE_FR].pwm = normal;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = normal;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = normal;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = normal;
-                    motor[TIRE_BL].dir = FOR;
                     break;
                 case 0:
-                    motor[TIRE_FR].pwm = fast;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast;
-                    motor[TIRE_BL].dir = FOR;
+                    motor[TIRE_FR].pwm = fast+55;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = fast+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = fast+50;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = fast+45;
+                    motor[TIRE_BL].dir = BACK;
                     mtc=true;
                     break;
                 case 255:
-                    motor[TIRE_FR].pwm = fast;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = slow;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = slow;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast;
-                    motor[TIRE_BL].dir = FOR;
+                    motor[TIRE_FR].pwm = fast+55;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = slow+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = slow+50;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = fast+45;
+                    motor[TIRE_BL].dir = BACK;
                     mtc=true;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = slow;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FR].pwm = slow+55;
+                    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 = FOR;
+                    motor[TIRE_BL].pwm = slow+45;
+                    motor[TIRE_BL].dir = BACK;
                     mtc=true;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = normal;
-                    motor[TIRE_FR].dir = BACK;
+                    motor[TIRE_FR].pwm = normal+55;
+                    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 = FOR;
+                    motor[TIRE_BL].pwm = normal+45;
+                    motor[TIRE_BL].dir = BACK;
                     mtc=true;
                     break;
                 case 1:
-                    motor[TIRE_FR].pwm = slow;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast;
-                    motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = slow;
-                    motor[TIRE_BL].dir = FOR;
+                    motor[TIRE_FR].pwm = slow+55;
+                    motor[TIRE_FR].dir = FOR;
+                    motor[TIRE_FL].pwm = fast+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = fast+50;
+                    motor[TIRE_BR].dir = FOR;
+                    motor[TIRE_BL].pwm = slow+45;
+                    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 = FOR;
-                    motor[TIRE_BR].pwm = slow;
-                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_FL].pwm = slow+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = slow+50;
+                    motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = 0;
                     motor[TIRE_BL].dir = FREE;
                     mtc=true;
@@ -1086,60 +605,906 @@
                 case 2:
                     motor[TIRE_FR].pwm = 0;
                     motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_FL].pwm = normal;
-                    motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = normal;
-                    motor[TIRE_BR].dir = BACK;
+                    motor[TIRE_FL].pwm = normal+50;
+                    motor[TIRE_FL].dir = BACK;
+                    motor[TIRE_BR].pwm = normal+50;
+                    motor[TIRE_BR].dir = FOR;
                     motor[TIRE_BL].pwm = 0;
                     motor[TIRE_BL].dir = FREE;
                     mtc=true;
                     break;
             }
-            if(g[0]!=98&&mtc==true) {
+            if(g[1]!=98&&mtc==true) {
                 switch(g[0]) {
                     case 0:
                         mtc=false;
                         break;
                     case 255:
-                        motor[TIRE_BR].pwm += 5;
+                        motor[TIRE_FL].pwm += 3;
                         mtc=false;
                         break;
                     case 253:
-                        motor[TIRE_BR].pwm += 10;
+                        motor[TIRE_FL].pwm += 6;
                         mtc=false;
                         break;
                     case 254:
-                        motor[TIRE_BR].pwm += 20;
-                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_FL].pwm += 9;
+                        motor[TIRE_FR].pwm = 0;
                         mtc=false;
                         break;
                     case 1:
-                        motor[TIRE_BL].pwm += 5;
+                        motor[TIRE_FR].pwm += 3;
                         mtc=false;
                         break;
                     case 3:
-                        motor[TIRE_BL].pwm += 10;
+                        motor[TIRE_FR].pwm += 6;
                         mtc=false;
                         break;
                     case 2:
-                        motor[TIRE_BL].pwm += 20;
-                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_FR].pwm += 9;
+                        motor[TIRE_FL].pwm = 0;
                         mtc=false;
                         break;
                 }
             }
-        }
-    } else if(mode==23) {
-        if(YELLOW) {
+            break;
+        case 11:
+            motor[TIRE_FR].pwm = 25;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = normal;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = normal;
+            motor[TIRE_BR].dir = BRAKE;
+            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;
+            }
+            break;
+        case 20:
+            solenoid.solenoid2= 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;
+                        }
+                        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 = 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]) {
+                    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;
+                }
+            }
+            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 = 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=22;
+                    fast=60;
+                    normal=40;
+                    slow=25;
+                }
+            }
+            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 = 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;
+                }
+                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[0]==98) {
+                    slow=25;
+                    normal=30;
+                    fast=40;
+                }
+                if(g[3]==0) {
+                    slow=25;
+                    normal=40;
+                    fast=60;
+                    mode=23;
+                }
+                switch(g[0]) {
+                    case 98:
+                        solenoid.solenoid2 = SOLENOID_OFF;
+                        fast=40;
+                        normal=30;
+                        slow=25;
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                    case 0:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                }
+                if(g[0]!=98&&mtc==true) {
+                    switch(g[0]) {
+                        case 0:
+                            mtc=false;
+                            break;
+                        case 255:
+                            motor[TIRE_BR].pwm += 5;
+                            mtc=false;
+                            break;
+                        case 253:
+                            motor[TIRE_BR].pwm += 10;
+                            mtc=false;
+                            break;
+                        case 254:
+                            motor[TIRE_BR].pwm += 20;
+                            motor[TIRE_BL].pwm = 0;
+                            mtc=false;
+                            break;
+                        case 1:
+                            motor[TIRE_BL].pwm += 5;
+                            mtc=false;
+                            break;
+                        case 3:
+                            motor[TIRE_BL].pwm += 10;
+                            mtc=false;
+                            break;
+                        case 2:
+                            motor[TIRE_BL].pwm += 20;
+                            motor[TIRE_BR].pwm = 0;
+                            mtc=false;
+                            break;
+                    }
+                }
+            }
+            break;
+        case 23:
+            if(YELLOW) {
+                switch(g[4]) {
+                    case 98:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = FOR;
+                        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 = BACK;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = FOR;
+                        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 = fast-5;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = slow-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = slow-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = fast-5;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = normal-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = normal-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast-5;
+                        motor[TIRE_BL].dir = FOR;
+                        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 = slow-5;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = slow-5;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = normal-5;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast-5;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast-5;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = normal-5;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                }
+                if(g[0]==98) {
+                    fast=20;
+                    normal=15;
+                    slow=10;
+                }
+                if(g[2]==254&&countss>=100000) {
+                    mode=24;
+                    solenoid.solenoid2 = SOLENOID_OFF;
+                    countss=0;
+                    fast=60;
+                    normal=40;
+                    slow=20;
+                }
+            } else {
+                if(UPLS==true||DOLS==true) {
+                    if(UPLS==true||DOLS==true) {
+                        motor[MOTOR_0].pwm = 200;
+                        motor[MOTOR_0].dir = BRAKE;
+                        fast=60;
+                        normal=40;
+                        slow=20;
+                        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_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;
+                }
+                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 = 20;
+                        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[0]==98&&countss>=20000) {
+                    mode=31;
+                    countss=0;
+                }
+                switch(g[0]) {
+                    case 98:
+                        fast=40;
+                        normal=30;
+                        slow=25;
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = FOR;
+                        break;
+                    case 0:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 255:
+                        motor[TIRE_FR].pwm = fast;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = fast;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 253:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 254:
+                        motor[TIRE_FR].pwm = normal;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = 0;
+                        motor[TIRE_FL].dir = FREE;
+                        motor[TIRE_BR].pwm = 0;
+                        motor[TIRE_BR].dir = FREE;
+                        motor[TIRE_BL].pwm = normal;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 1:
+                        motor[TIRE_FR].pwm = slow;
+                        motor[TIRE_FR].dir = BACK;
+                        motor[TIRE_FL].pwm = fast;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = fast;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = slow;
+                        motor[TIRE_BL].dir = FOR;
+                        mtc=true;
+                        break;
+                    case 3:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = slow;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = slow;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                    case 2:
+                        motor[TIRE_FR].pwm = 0;
+                        motor[TIRE_FR].dir = FREE;
+                        motor[TIRE_FL].pwm = normal;
+                        motor[TIRE_FL].dir = FOR;
+                        motor[TIRE_BR].pwm = normal;
+                        motor[TIRE_BR].dir = BACK;
+                        motor[TIRE_BL].pwm = 0;
+                        motor[TIRE_BL].dir = FREE;
+                        mtc=true;
+                        break;
+                }
+                if(g[1]!=98&&mtc==true) {
+                    switch(g[1]) {
+                        case 0:
+                            mtc=false;
+                            break;
+                        case 255:
+                            motor[TIRE_BR].pwm += 5;
+                            mtc=false;
+                            break;
+                        case 253:
+                            motor[TIRE_BR].pwm += 10;
+                            mtc=false;
+                            break;
+                        case 254:
+                            motor[TIRE_BR].pwm += 20;
+                            motor[TIRE_BL].pwm = 0;
+                            mtc=false;
+                            break;
+                        case 1:
+                            motor[TIRE_BL].pwm += 5;
+                            mtc=false;
+                            break;
+                        case 3:
+                            motor[TIRE_BL].pwm += 10;
+                            mtc=false;
+                            break;
+                        case 2:
+                            motor[TIRE_BL].pwm += 20;
+                            motor[TIRE_BR].pwm = 0;
+                            mtc=false;
+                            break;
+                    }
+                }
+            }
+            break;
+        case 25:
+            motor[TIRE_FR].pwm = 100;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 100;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 100;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 100;
+            motor[TIRE_BL].dir = BRAKE;
+            if(DOLS==true||UPLS==true) {
+                if(DOLS==true||UPLS==true) {
+                    motor[MOTOR_0].pwm = 200;
+                    motor[MOTOR_0].dir = BRAKE;
+                    countss=0;
+                    mode=26;
+                }
+            } else {
+                motor[MOTOR_0].pwm = 100;
+                motor[MOTOR_0].dir = BACK;
+            }
+            break;
+        case 26:
             switch(g[4]) {
                 case 98:
-                    motor[TIRE_FR].pwm = fast-5;
+                    motor[TIRE_FR].pwm = 60;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast-5;
+                    motor[TIRE_FL].pwm = 60;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast-5;
+                    motor[TIRE_BR].pwm = 60;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast-5;
+                    motor[TIRE_BL].pwm = 60;
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 99:
@@ -1153,13 +1518,13 @@
                     motor[TIRE_BL].dir = FREE;
                     break;
                 case 0:
-                    motor[TIRE_FR].pwm = fast-5;
+                    motor[TIRE_FR].pwm = 60;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast-5;
+                    motor[TIRE_FL].pwm = 60;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast-5;
+                    motor[TIRE_BR].pwm = 60;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast-5;
+                    motor[TIRE_BL].pwm = 60;
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 255:
@@ -1173,23 +1538,23 @@
                     motor[TIRE_BL].dir = FREE;
                     break;
                 case 253:
-                    motor[TIRE_FR].pwm = fast-5;
+                    motor[TIRE_FR].pwm = 60;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = slow-5;
+                    motor[TIRE_FL].pwm = 25;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = slow-5;
+                    motor[TIRE_BR].pwm = 25;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast-5;
+                    motor[TIRE_BL].pwm = 60;
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 254:
-                    motor[TIRE_FR].pwm = fast-5;
+                    motor[TIRE_FR].pwm = 60;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = normal-5;
+                    motor[TIRE_FL].pwm = 40;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = normal-5;
+                    motor[TIRE_BR].pwm = 40;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast-5;
+                    motor[TIRE_BL].pwm = 60;
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 1:
@@ -1203,185 +1568,232 @@
                     motor[TIRE_BL].dir = FREE;
                     break;
                 case 3:
-                    motor[TIRE_FR].pwm = slow-5;
+                    motor[TIRE_FR].pwm = 25;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast-5;
+                    motor[TIRE_FL].pwm = 60;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast-5;
+                    motor[TIRE_BR].pwm = 60;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = slow-5;
+                    motor[TIRE_BL].pwm = 25;
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 2:
-                    motor[TIRE_FR].pwm = normal-5;
+                    motor[TIRE_FR].pwm = 40;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast-5;
+                    motor[TIRE_FL].pwm = 60;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast-5;
+                    motor[TIRE_BR].pwm = 60;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = normal-5;
+                    motor[TIRE_BL].pwm = 40;
                     motor[TIRE_BL].dir = FOR;
                     break;
             }
             if(g[0]==98) {
-                fast=30;
-                normal=25;
-                slow=20;
-            }
-            if(g[2]==254&&countss>=100000) {
-                mode=24;
-                solenoid.solenoid2 = SOLENOID_OFF;
-                countss=0;
-                fast=60;
-                normal=40;
-                slow=20;
+                mode=31;
             }
-        } else {
-            if(UPLS==true||DOLS==true) {
-                if(UPLS==true||DOLS==true) {
-                    motor[MOTOR_0].pwm = 200;
-                    motor[MOTOR_0].dir = BRAKE;
-                    fast=60;
-                    normal=40;
-                    slow=20;
-                    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;
+            break;
+        case 31:
+            motor[TIRE_FR].pwm = 45;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 255;
             motor[TIRE_FL].dir = BRAKE;
-            motor[TIRE_BR].pwm = 100;
+            motor[TIRE_BR].pwm = 255;
             motor[TIRE_BR].dir = BRAKE;
-            motor[TIRE_BL].pwm = 100;
-            motor[TIRE_BL].dir = BRAKE;
-        }
-    } else if(mode==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_BR].pwm = 30;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 30;
-                motor[TIRE_BL].dir = BACK;
+            motor[TIRE_BL].pwm = 45;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]==0) {
+                cross=0;
+                countss=0;
+                mode=40;
+            }
+            break;
+        case 40:
+            if(LIF||LIB) {
+                mode=42;
             }
-            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;
-            }
-            switch(g[2]) {
+            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;
+                    if(YELLOW) {
+                        if(cross==1) {
+                            mode=41;
+                            countss=0;
+                            cross=0;
+                        }
+                    } else {
+                        if(cross==3) {
+                            mode=41;
+                            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+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 = 30;
+                    motor[TIRE_FR].pwm = slow+15;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 20;
-                    motor[TIRE_BR].dir = FREE;
+                    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 = 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+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 = 30;
-                    motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = BACK;
+                    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 = 20;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 30;
+                    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 = 20;
-                    motor[TIRE_FR].dir = FREE;
-                    motor[TIRE_BR].pwm = 40;
-                    motor[TIRE_BR].dir = FOR;
+                    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 = 30;
-                    motor[TIRE_FR].dir = FOR;
-                    motor[TIRE_BR].pwm = 30;
-                    motor[TIRE_BR].dir = FOR;
+                    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;
             }
-            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;
+            if(g[2]!=98&&mtc2==true) {
+                switch(g[2]) {
+                    case 0:
+                        mtc2=false;
+                        break;
+                    case 255:
+                        motor[TIRE_FL].pwm += 5;
+                        mtc2=false;
+                        break;
+                    case 253:
+                        motor[TIRE_FL].pwm += 10;
+                        mtc2=false;
+                        break;
+                    case 254:
+                        motor[TIRE_FL].pwm += 20;
+                        motor[TIRE_BL].pwm = 0;
+                        mtc2=false;
+                        break;
+                    case 1:
+                        motor[TIRE_BL].pwm += 5;
+                        mtc2=false;
+                        break;
+                    case 3:
+                        motor[TIRE_BL].pwm += 10;
+                        mtc2=false;
+                        break;
+                    case 2:
+                        motor[TIRE_BL].pwm += 20;
+                        motor[TIRE_FL].pwm = 0;
+                        mtc2=false;
+                        break;
+                }
+            }
+            break;
+        case 41:
+            if(LIF||LIB) {
+                mode=42;
+            }
+            motor[TIRE_FR].pwm = 40;
+            motor[TIRE_FR].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 = 40;
+            motor[TIRE_BL].dir = FOR;
+            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 = 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;
+            }
+            break;
+        case 70:
+            switch(g[0]) {
+                case 99:
+                    if(countss>=100000) {
+                        mode=100;
+                    }
                     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=25;
-            }
-        } else {
-            if(g[0]==98&&countss>=40000) {
-                mode=31;
-                countss=0;
-            }
-            switch(g[0]) {
                 case 98:
-                    fast=40;
-                    normal=30;
-                    slow=25;
                     motor[TIRE_FR].pwm = normal;
                     motor[TIRE_FR].dir = BACK;
                     motor[TIRE_FL].pwm = normal;
@@ -1392,13 +1804,13 @@
                     motor[TIRE_BL].dir = FOR;
                     break;
                 case 0:
-                    motor[TIRE_FR].pwm = fast;
+                    motor[TIRE_FR].pwm = 100;
                     motor[TIRE_FR].dir = BACK;
-                    motor[TIRE_FL].pwm = fast;
+                    motor[TIRE_FL].pwm = 100;
                     motor[TIRE_FL].dir = FOR;
-                    motor[TIRE_BR].pwm = fast;
+                    motor[TIRE_BR].pwm = 100;
                     motor[TIRE_BR].dir = BACK;
-                    motor[TIRE_BL].pwm = fast;
+                    motor[TIRE_BL].pwm = 100;
                     motor[TIRE_BL].dir = FOR;
                     mtc=true;
                     break;
@@ -1502,453 +1914,28 @@
                         break;
                 }
             }
-        }
-    } else if(mode==25) {
-        motor[TIRE_FR].pwm = 100;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 100;
-        motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 100;
-        motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 100;
-        motor[TIRE_BL].dir = BRAKE;
-        if(DOLS==true||UPLS==true) {
-            if(DOLS==true||UPLS==true) {
-                motor[MOTOR_0].pwm = 200;
-                motor[MOTOR_0].dir = BRAKE;
-                countss=0;
-                mode=26;
-            }
-        } else {
-            motor[MOTOR_0].pwm = 100;
-            motor[MOTOR_0].dir = BACK;
-        }
-    } else if(mode==26) {
-        switch(g[4]) {
-            case 98:
-                motor[TIRE_FR].pwm = 60;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = FOR;
-                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 = 60;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = FOR;
-                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 = 60;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 20;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 20;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = FOR;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = 60;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 40;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 40;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = FOR;
-                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 = 40;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 40;
-                motor[TIRE_BL].dir = FOR;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = 40;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 40;
-                motor[TIRE_BL].dir = FOR;
-                break;
-        }
-        if(g[0]==98) {
-            mode=31;
-        }
-    } else if(mode==31) {
-        motor[TIRE_FR].pwm = 45;
-        motor[TIRE_FR].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 = 45;
-        motor[TIRE_BL].dir = FOR;
-        if(g[2]==0) {
-            cross=0;
-            countss=0;
-            mode=40;
-        }
-    } else if(mode==40) {
-        if(LIF||LIB) {
-            mode=42;
-        }
-        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>10000) {
-                    cross++;
-                    countss=0;
-                }
-                mtc2=true;
-                if(YELLOW) {
-                    if(cross==1) {
-                        mode=41;
-                        countss=0;
-                        cross=0;
-                    }
-                } else {
-                    if(cross==3) {
-                        mode=41;
-                        countss=0;
-                        cross=0;
-                    }
-                }
-                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_FL].pwm += 5;
-                    mtc2=false;
-                    break;
-                case 253:
-                    motor[TIRE_FL].pwm += 10;
-                    mtc2=false;
-                    break;
-                case 254:
-                    motor[TIRE_FL].pwm += 20;
-                    motor[TIRE_BL].pwm = 0;
-                    mtc2=false;
-                    break;
-                case 1:
-                    motor[TIRE_BL].pwm += 5;
-                    mtc2=false;
-                    break;
-                case 3:
-                    motor[TIRE_BL].pwm += 10;
-                    mtc2=false;
-                    break;
-                case 2:
-                    motor[TIRE_BL].pwm += 20;
-                    motor[TIRE_FL].pwm = 0;
-                    mtc2=false;
-                    break;
-            }
-        }
-    } else if(mode==41) {
-        if(LIF||LIB) {
-            mode=42;
-        }
-        motor[TIRE_FR].pwm = 30;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = FREE;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = 30;
-        motor[TIRE_BL].dir = FOR;
-        if(g[1]==0) {
-            countss=0;
-            mode=70;
-            normal=60;
-            fast=70;
-            slow=40;
-        }
-    } else if(mode==42) {
-        motor[TIRE_FR].pwm = 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[0]) {
-            case 99:
-                if(countss>=100000) {
-                    mode=100;
-                }
-                break;
-            case 98:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = FOR;
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = 80;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 80;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 80;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 80;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = fast;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-        }
-        if(g[1]!=98&&mtc==true) {
-            switch(g[1]) {
-                case 0:
-                    mtc=false;
-                    break;
-                case 255:
-                    motor[TIRE_BR].pwm += 5;
-                    mtc=false;
-                    break;
-                case 253:
-                    motor[TIRE_BR].pwm += 10;
-                    mtc=false;
-                    break;
-                case 254:
-                    motor[TIRE_BR].pwm += 20;
-                    motor[TIRE_BL].pwm = 0;
-                    mtc=false;
-                    break;
-                case 1:
-                    motor[TIRE_BL].pwm += 5;
-                    mtc=false;
-                    break;
-                case 3:
-                    motor[TIRE_BL].pwm += 10;
-                    mtc=false;
-                    break;
-                case 2:
-                    motor[TIRE_BL].pwm += 20;
-                    motor[TIRE_BR].pwm = 0;
-                    mtc=false;
-                    break;
-            }
-        }
-    } else if(mode==100) {
-        motor[TIRE_FR].pwm = 50;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 50;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 50;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 50;
-        motor[TIRE_BL].dir = FOR;
-    } else {
+            break;
+        case 100:
+            motor[TIRE_FR].pwm = 50;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 50;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 50;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 50;
+            motor[TIRE_BL].dir = FOR;
+            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;
     }
-
 }
 #endif