![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aa
Revision 53:694dbd7dfa0e, committed 2019-11-15
- Comitter:
- yabahiro
- Date:
- Fri Nov 15 11:49:03 2019 +0000
- Parent:
- 52:4394a1debceb
- Commit message:
- ugoku
Changed in this revision
System/Process/Process.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 4394a1debceb -r 694dbd7dfa0e System/Process/Process.cpp --- a/System/Process/Process.cpp Wed Nov 13 21:21:44 2019 +0000 +++ b/System/Process/Process.cpp Fri Nov 15 11:49:03 2019 +0000 @@ -213,7 +213,7 @@ SystemProcessInitialize(); while(1) { - //pc.printf("4:%d\n\r",g[4]); + //pc.printf("3:%d\n\r",g[3]); if(EMC==0) { fast=40; @@ -469,7 +469,7 @@ static void Process2() { //Oh So 青ゾーン - LED::LED::TapeledMode(2); + LED::LED::TapeledMode(4); countss++; switch(mode) { case 1: @@ -649,13 +649,13 @@ } break; case 11: - motor[TIRE_FR].pwm = 25; + motor[TIRE_FR].pwm = normal; 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_FL].pwm = 15; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 15; + motor[TIRE_BR].dir = BACK; + 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; @@ -913,42 +913,42 @@ } switch(g[3]) { case 0: - motor[TIRE_FL].pwm = 20; + motor[TIRE_FL].pwm = 25; 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].pwm = 25; 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].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].pwm = 35; 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].pwm = 35; 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].pwm = 45; 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].pwm = 35; motor[TIRE_FL].dir = FOR; motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = FOR; @@ -962,7 +962,7 @@ if(g[0]==98) { slow=25; normal=30; - fast=40; + fast=30; } if(g[3]==0) { slow=25; @@ -978,7 +978,7 @@ slow=25; motor[TIRE_FR].pwm = normal; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].pwm = normal+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = BACK; @@ -988,7 +988,7 @@ case 0: motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].pwm = fast+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = BACK; @@ -999,7 +999,7 @@ case 255: motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].pwm = slow+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = BACK; @@ -1032,7 +1032,7 @@ case 1: motor[TIRE_FR].pwm = slow; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].pwm = fast+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = BACK; @@ -1043,7 +1043,7 @@ case 3: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].pwm = slow+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = BACK; @@ -1054,7 +1054,7 @@ case 2: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].pwm = normal+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = BACK; @@ -1104,7 +1104,7 @@ case 98: motor[TIRE_FR].pwm = fast-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast-5; motor[TIRE_BR].dir = BACK; @@ -1124,7 +1124,7 @@ case 0: motor[TIRE_FR].pwm = fast-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast-5; motor[TIRE_BR].dir = BACK; @@ -1144,7 +1144,7 @@ case 253: motor[TIRE_FR].pwm = fast-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow-5; + motor[TIRE_FL].pwm = slow; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = slow-5; motor[TIRE_BR].dir = BACK; @@ -1154,7 +1154,7 @@ case 254: motor[TIRE_FR].pwm = fast-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal-5; + motor[TIRE_FL].pwm = normal; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = normal-5; motor[TIRE_BR].dir = BACK; @@ -1174,7 +1174,7 @@ case 3: motor[TIRE_FR].pwm = slow-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast-5; motor[TIRE_BR].dir = BACK; @@ -1184,7 +1184,7 @@ case 2: motor[TIRE_FR].pwm = normal-5; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast-5; motor[TIRE_BR].dir = BACK; @@ -1212,7 +1212,7 @@ motor[MOTOR_0].dir = BRAKE; fast=60; normal=40; - slow=20; + slow=30; countss=0; mode=24; } @@ -1315,25 +1315,25 @@ motor[TIRE_BL].dir = BACK; break; case 254: - motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].pwm = 35; 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].pwm = 35; motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 20; + motor[TIRE_BL].pwm = 25; motor[TIRE_BL].dir = FREE; break; case 3: - motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].pwm = 45; 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].pwm = 35; motor[TIRE_FL].dir = FOR; motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = FOR; @@ -1343,18 +1343,18 @@ mode=25; } } else { - if(g[0]==98&&countss>=20000) { + if(g[0]==98&&countss>=40000) { mode=31; + fast=60; + normal=40; + slow=30; 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].pwm = normal+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = BACK; @@ -1364,7 +1364,7 @@ case 0: motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].pwm = fast+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = BACK; @@ -1375,7 +1375,7 @@ case 255: motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].pwm = slow+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = BACK; @@ -1408,7 +1408,7 @@ case 1: motor[TIRE_FR].pwm = slow; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].pwm = fast+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = BACK; @@ -1419,7 +1419,7 @@ case 3: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].pwm = slow+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = BACK; @@ -1430,7 +1430,7 @@ case 2: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].pwm = normal+5; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = BACK; @@ -1496,11 +1496,14 @@ } break; case 26: + if(g[0]==98) { + mode=31; + } switch(g[4]) { case 98: motor[TIRE_FR].pwm = 60; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].pwm = 65; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 60; motor[TIRE_BR].dir = BACK; @@ -1520,7 +1523,7 @@ case 0: motor[TIRE_FR].pwm = 60; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].pwm = 65; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 60; motor[TIRE_BR].dir = BACK; @@ -1540,7 +1543,7 @@ case 253: motor[TIRE_FR].pwm = 60; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 25; + motor[TIRE_FL].pwm = 30; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 25; motor[TIRE_BR].dir = BACK; @@ -1550,7 +1553,7 @@ case 254: motor[TIRE_FR].pwm = 60; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].pwm = 45; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 40; motor[TIRE_BR].dir = BACK; @@ -1570,7 +1573,7 @@ case 3: motor[TIRE_FR].pwm = 25; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].pwm = 65; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 60; motor[TIRE_BR].dir = BACK; @@ -1580,7 +1583,7 @@ case 2: motor[TIRE_FR].pwm = 40; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].pwm = 65; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 60; motor[TIRE_BR].dir = BACK; @@ -1588,38 +1591,35 @@ motor[TIRE_BL].dir = FOR; break; } - if(g[0]==98) { - mode=31; - } break; case 31: - motor[TIRE_FR].pwm = 45; + motor[TIRE_FR].pwm = 30; 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_FL].pwm = 5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = FOR; if(g[2]==0) { cross=0; countss=0; mode=40; + fast=60; + normal=40; + slow=30; } break; case 40: - if(LIF||LIB) { - mode=42; - } switch(g[3]) { case 98: - motor[TIRE_FR].pwm = normal+15; + motor[TIRE_FR].pwm = normal; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal+15; + motor[TIRE_FL].pwm = normal; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+15; + motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = normal+15; + motor[TIRE_BL].pwm = normal; motor[TIRE_BL].dir = FOR; if(countss>30000) { cross++; @@ -1634,6 +1634,9 @@ } } else { if(cross==3) { + fast=60; + normal=40; + slow=30; mode=41; countss=0; cross=0; @@ -1641,33 +1644,33 @@ } break; case 0: - motor[TIRE_FR].pwm = fast+15; + motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+15; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast+15; + motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast+15; + motor[TIRE_BL].pwm = fast; motor[TIRE_BL].dir = FOR; mtc2=true; break; case 255: - motor[TIRE_FR].pwm = slow+15; + motor[TIRE_FR].pwm = slow; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+15; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast+15; + motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = slow+15; + motor[TIRE_BL].pwm = slow; motor[TIRE_BL].dir = FOR; mtc2=true; break; case 253: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow+15; + motor[TIRE_FL].pwm = slow; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+15; + motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = 0; motor[TIRE_BL].dir = FREE; @@ -1676,44 +1679,44 @@ case 254: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal+15; + motor[TIRE_FL].pwm = normal; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+15; + motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = 0; motor[TIRE_BL].dir = FREE; mtc2=true; break; case 1: - motor[TIRE_FR].pwm = fast+15; + motor[TIRE_FR].pwm = slow; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow+15; + motor[TIRE_FL].pwm = slow; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+15; + motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast+15; + motor[TIRE_BL].pwm = fast; motor[TIRE_BL].dir = FOR; mtc2=true; break; case 3: - motor[TIRE_FR].pwm = slow+15; + 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+15; + motor[TIRE_BL].pwm = slow; motor[TIRE_BL].dir = FOR; mtc2=true; break; case 2: - motor[TIRE_FR].pwm = normal+15; + 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+15; + motor[TIRE_BL].pwm = normal; motor[TIRE_BL].dir = FOR; mtc2=true; break; @@ -1753,23 +1756,25 @@ } break; case 41: - if(LIF||LIB) { - mode=42; - } - motor[TIRE_FR].pwm = 40; + /* + if(LIF||LIB) { + mode=42; + } + */ + motor[TIRE_FR].pwm = 30; 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_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=80; - fast=100; - slow=60; + normal=60; + fast=70; + slow=40; } break; case 42: @@ -1804,13 +1809,13 @@ motor[TIRE_BL].dir = FOR; break; case 0: - motor[TIRE_FR].pwm = 100; + motor[TIRE_FR].pwm = 80; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 100; + motor[TIRE_FL].pwm = 80; motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = 100; + motor[TIRE_BR].pwm = 80; motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 100; + motor[TIRE_BL].pwm = 80; motor[TIRE_BL].dir = FOR; mtc=true; break; @@ -1945,1010 +1950,688 @@ //Clearly 赤ゾーン LED::LED::TapeledMode(3); countss++; - switch(mode) { - case 1: - fast=40; - normal=30; - slow=20; - motor[TIRE_FR].pwm = normal+20; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal+20; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = normal+20; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = normal+20; - motor[TIRE_BL].dir = FOR; - if(g[2]==0&&countss>=100000) { - countss=100000; - cross=0; - mode=3; - } - break; - case 3: - motor[TIRE_FR].pwm = 255; - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].pwm = 30; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = 30; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 255; - motor[TIRE_BL].dir = BRAKE; - if(g[0]==0) { - mode=10; - countss=100000; - } - break; - case 10: - switch(g[0]) { - case 98: - motor[TIRE_FR].pwm = normal+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = normal+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = normal+50; - motor[TIRE_BL].dir = FOR; - mtc=true; - if(countss>80000) { - cross++; - countss=0; - } - if(cross==2) { - fast = 10; - normal = 10; - slow = 5; - } + if(mode==1) {//スタートゾーンから白線検知 + fast=40; + normal=30; + slow=20; + motor[TIRE_FR].pwm = normal+20; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = normal+20; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = normal+20; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = normal+20; + motor[TIRE_BL].dir = FOR; + if(g[2]==0&&countss>=100000) { + countss=100000; + cross=0; + mode=3; + } + } else if(mode==3) { //横ライントレースから縦ライントレースへ + motor[TIRE_FR].pwm = 255; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 255; + motor[TIRE_BL].dir = BRAKE; + if(g[0]==0) { + mode=10; + countss=100000; + } + } else if(mode==10) { //縦ライントレース + switch(g[0]) { + case 98: + motor[TIRE_FR].pwm = normal+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = normal+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = normal+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = normal+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + if(countss>80000) { + cross++; + countss=0; + } + if(cross==2) { + fast = 10; + normal = 10; + slow = 5; + } + + if(cross==3) { + fast=40; + normal=30; + slow=20; + mode=11; + countss=0; + cross=0; - if(cross==3) { - fast=40; - normal=30; - slow=20; - mode=11; - countss=0; - cross=0; - - } - break; + } + break; + case 0: + motor[TIRE_FR].pwm = fast+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = fast+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = fast+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = fast+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 255: + motor[TIRE_FR].pwm = fast+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = slow+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = slow+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = fast+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 253: + motor[TIRE_FR].pwm = slow+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = slow+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 254: + motor[TIRE_FR].pwm = normal+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = normal+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 1: + motor[TIRE_FR].pwm = slow+50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = fast+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = fast+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = slow+50; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 3: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = slow+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = slow+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + case 2: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = normal+50; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = normal+50; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + } + if(g[0]!=98&&mtc==true) { + switch(g[1]) { case 0: - motor[TIRE_FR].pwm = fast+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = fast+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = fast+50; - motor[TIRE_BL].dir = FOR; - mtc=true; + mtc=false; break; case 255: - motor[TIRE_FR].pwm = fast+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = slow+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = fast+50; - motor[TIRE_BL].dir = FOR; - mtc=true; + motor[TIRE_BR].pwm += 3; + mtc=false; break; case 253: - motor[TIRE_FR].pwm = slow+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = slow+50; - motor[TIRE_BL].dir = FOR; - mtc=true; + motor[TIRE_BR].pwm += 6; + mtc=false; break; case 254: - motor[TIRE_FR].pwm = normal+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = normal+50; - motor[TIRE_BL].dir = FOR; - mtc=true; + motor[TIRE_BR].pwm += 9; + motor[TIRE_BL].pwm = 0; + mtc=false; break; case 1: - motor[TIRE_FR].pwm = slow+50; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = fast+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = slow+50; - motor[TIRE_BL].dir = FOR; - mtc=true; + motor[TIRE_BL].pwm += 3; + mtc=false; break; case 3: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = slow+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; + motor[TIRE_BL].pwm += 6; + mtc=false; break; case 2: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal+50; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = normal+50; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; + motor[TIRE_BL].pwm += 9; + motor[TIRE_BR].pwm = 0; + mtc=false; break; } - if(g[0]!=98&&mtc==true) { - switch(g[1]) { - case 0: - mtc=false; - break; - case 255: - motor[TIRE_BR].pwm += 3; - mtc=false; - break; - case 253: - motor[TIRE_BR].pwm += 6; - mtc=false; - break; - case 254: - motor[TIRE_BR].pwm += 9; - motor[TIRE_BL].pwm = 0; - mtc=false; - break; - case 1: - motor[TIRE_BL].pwm += 3; - mtc=false; - break; - case 3: - motor[TIRE_BL].pwm += 6; - mtc=false; - break; - case 2: - motor[TIRE_BL].pwm += 9; - motor[TIRE_BR].pwm = 0; - mtc=false; - break; + } + } else if(mode==11) { + motor[TIRE_FR].pwm = 25; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = normal; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 25; + motor[TIRE_BL].dir = BACK; + if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { + countss=0; + cross=0; + mode=20; + } + } else if(mode==20) { + solenoid.solenoid4= SOLENOID_ON; + switch(g[2]) { + case 98: + motor[TIRE_FR].pwm = normal+20; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = normal+20; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = normal+20; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = normal+20; + motor[TIRE_BL].dir = BACK; + if(countss>30000) { + cross++; + countss=0; + } + mtc2=true; + + if(YELLOW) { + if(cross==1) { + fast=20; + normal=15; + slow=15; + } else if(cross==2) { + mode=21; + countss=0; + cross=0; + } + } else { + if(cross==3) { + mode=21; + countss=0; + cross=0; + } } - } - break; - case 11: - motor[TIRE_FR].pwm = 25; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = normal; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = normal; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 25; - motor[TIRE_BL].dir = BACK; - if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) { - countss=0; - cross=0; - mode=20; - } - break; - case 20: - solenoid.solenoid4= SOLENOID_ON; - switch(g[2]) { - case 98: - motor[TIRE_FR].pwm = normal+20; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = normal+20; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = normal+20; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = normal+20; - motor[TIRE_BL].dir = BACK; - if(countss>30000) { - cross++; - countss=0; - } - mtc2=true; - - if(YELLOW) { - if(cross==1) { - fast=20; - normal=15; - slow=15; - } else if(cross==2) { - mode=21; - countss=0; - cross=0; - } - } else { - if(cross==3) { - mode=21; - countss=0; - cross=0; - } - } - break; + 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: - 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; + mtc2=false; 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; + motor[TIRE_BL].pwm += 5; + mtc2=false; + break; + case 253: + motor[TIRE_BL].pwm += 10; + mtc2=false; + break; + case 254: + motor[TIRE_BL].pwm += 15; + motor[TIRE_FL].pwm = 0; + mtc2=false; + break; + case 1: + motor[TIRE_FL].pwm += 5; + mtc2=false; + break; + case 3: + motor[TIRE_FL].pwm += 10; + mtc2=false; + break; + case 2: + motor[TIRE_FL].pwm += 15; + motor[TIRE_BL].pwm = 0; + mtc2=false; + break; + } + } + } + if(YELLOW) { + if(mode==21) { + motor[TIRE_FR].pwm = 20; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 20; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 20; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 20; + motor[TIRE_BL].dir = FOR; + if(g[3]==98&&countss>=10000) { + mode=22; + countss=0; + fast=60; + normal=40; + slow=20; + } + } else if(mode==22) { + if(g[4]==3||g[4]==2) { + motor[TIRE_FR].pwm = 30; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BL].dir = BACK; + } + if(g[4]==253||g[4]==254) { + motor[TIRE_FR].pwm = 30; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BL].dir = FOR; + } + 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 = 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; + 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(mode==23) { + switch(g[4]) { + case 98: + motor[TIRE_FR].pwm = fast-5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast-5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast-5; + motor[TIRE_BL].dir = BACK; + break; + case 99: motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = 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].pwm = 0; + motor[TIRE_BL].dir = FREE; + break; + case 0: + motor[TIRE_FR].pwm = fast-5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast-5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast-5; + motor[TIRE_BL].dir = BACK; + break; + case 255: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + break; + case 253: + motor[TIRE_FR].pwm = slow+5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast-5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = slow+5; motor[TIRE_BL].dir = BACK; - mtc2=true; + break; + case 254: + motor[TIRE_FR].pwm = normal+5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast-5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast-5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = normal+5; + motor[TIRE_BL].dir = BACK; + break; + case 1: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + break; + case 3: + motor[TIRE_FR].pwm = fast-5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = slow+5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast-5; + motor[TIRE_BL].dir = BACK; + break; + case 2: + motor[TIRE_FR].pwm = fast-5; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = normal+5; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast-5; + motor[TIRE_BL].dir = BACK; break; } - if(g[2]!=98&&mtc2==true) { - switch(g[3]) { - case 0: - mtc2=false; - break; - case 255: - motor[TIRE_BL].pwm += 5; - mtc2=false; - break; - case 253: - motor[TIRE_BL].pwm += 10; - mtc2=false; - break; - case 254: - motor[TIRE_BL].pwm += 15; - motor[TIRE_FL].pwm = 0; - mtc2=false; - break; - case 1: - motor[TIRE_FL].pwm += 5; - mtc2=false; - break; - case 3: - motor[TIRE_FL].pwm += 10; - mtc2=false; - break; - case 2: - motor[TIRE_FL].pwm += 15; - motor[TIRE_BL].pwm = 0; - mtc2=false; - break; - } + if(g[1]==98) { + fast=20; + normal=15; + slow=10; } - 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 { + if(g[3]==254&&countss>=100000) { + mode=24; + solenoid.solenoid4 = SOLENOID_OFF; + countss=0; + fast=60; + normal=40; + slow=20; + } + } else if(mode==24) { + if(g[4]==3||g[4]==2) { 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_FL].pwm = 30; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].dir = BACK; motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = BACK; - if(g[1]==0) { - mode=22; - fast=60; - normal=40; - slow=25; - } + } + if(g[4]==253||g[4]==254) { + motor[TIRE_FR].pwm = 30; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BL].dir = FOR; } - break; - case 22: - if(YELLOW) { - if(g[4]==3||g[4]==2) { + 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_FL].pwm = 30; - motor[TIRE_FL].dir = FOR; motor[TIRE_BR].pwm = 30; - motor[TIRE_BR].dir = BACK; + 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; - } - if(g[4]==253||g[4]==254) { - motor[TIRE_FR].pwm = 30; - motor[TIRE_FR].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_BR].pwm = 30; - motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BL].dir = BACK; + break; + case 1: + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].pwm = 25; + motor[TIRE_BL].dir = FREE; + break; + case 3: + motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].pwm = 25; + motor[TIRE_BL].dir = FREE; + break; + case 2: + motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].dir = FOR; motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = FOR; - } - switch(g[2]) { - case 0: - motor[TIRE_FR].pwm = 20; - motor[TIRE_FR].dir = FREE; - motor[TIRE_BR].pwm = 20; - motor[TIRE_BR].dir = FREE; - break; - case 255: - motor[TIRE_FR].pwm = 30; - motor[TIRE_FR].dir = BACK; - motor[TIRE_BR].pwm = 20; - motor[TIRE_BR].dir = FREE; - break; - case 253: - motor[TIRE_FR].pwm = 40; - motor[TIRE_FR].dir = BACK; - motor[TIRE_BR].pwm = 20; - motor[TIRE_BR].dir = FREE; - break; - case 254: - motor[TIRE_FR].pwm = 30; - motor[TIRE_FR].dir = BACK; - motor[TIRE_BR].pwm = 30; - motor[TIRE_BR].dir = BACK; - break; - case 1: - motor[TIRE_FR].pwm = 20; - motor[TIRE_FR].dir = FREE; - motor[TIRE_BR].pwm = 30; - motor[TIRE_BR].dir = FOR; - break; - case 3: - motor[TIRE_FR].pwm = 20; - motor[TIRE_FR].dir = FREE; - motor[TIRE_BR].pwm = 40; - motor[TIRE_BR].dir = FOR; - break; - case 2: - motor[TIRE_FR].pwm = 30; - motor[TIRE_FR].dir = FOR; - motor[TIRE_BR].pwm = 30; - motor[TIRE_BR].dir = FOR; - break; - } - switch(g[3]) { - case 0: - motor[TIRE_FL].pwm = 20; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BL].dir = FREE; - break; - case 255: - motor[TIRE_FL].pwm = 20; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BL].pwm = 30; - motor[TIRE_BL].dir = BACK; - case 253: - motor[TIRE_FL].pwm = 20; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BL].pwm = 40; - motor[TIRE_BL].dir = BACK; - break; - case 254: - motor[TIRE_FL].pwm = 30; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BL].pwm = 30; - motor[TIRE_BL].dir = BACK; - break; - case 1: - motor[TIRE_FL].pwm = 30; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BL].dir = FREE; - break; - case 3: - motor[TIRE_FL].pwm = 40; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BL].dir = FREE; - break; - case 2: - motor[TIRE_FL].pwm = 30; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 30; - motor[TIRE_BL].dir = FOR; - break; - } - if(g[3]==0&&g[2]==0) { - mode=23; - countss=0; - } - } else { - if(g[1]==98) { - slow=25; - normal=30; - fast=40; - } - if(g[3]==0) { - slow=25; - normal=40; - fast=60; - mode=23; - } - switch(g[1]) { - case 98: - solenoid.solenoid4 = SOLENOID_OFF; - fast=40; - normal=30; - slow=25; - motor[TIRE_FR].pwm = normal; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = normal; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = normal; - motor[TIRE_BL].dir = BACK; - break; - case 0: - motor[TIRE_FR].pwm = fast; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 255: - motor[TIRE_FR].pwm = fast; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = slow; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 253: - motor[TIRE_FR].pwm = slow; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = slow; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 254: - motor[TIRE_FR].pwm = normal; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = normal; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 1: - motor[TIRE_FR].pwm = slow; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = slow; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 3: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; - break; - case 2: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; - break; - } - if(g[1]!=98&&mtc==true) { - switch(g[0]) { - case 0: - mtc=false; - break; - case 255: - motor[TIRE_FL].pwm += 3; - mtc=false; - break; - case 253: - motor[TIRE_FL].pwm += 6; - mtc=false; - break; - case 254: - motor[TIRE_FL].pwm += 9; - motor[TIRE_FR].pwm = 0; - mtc=false; - break; - case 1: - motor[TIRE_FR].pwm += 3; - mtc=false; - break; - case 3: - motor[TIRE_FR].pwm += 6; - mtc=false; - break; - case 2: - motor[TIRE_FR].pwm += 9; - motor[TIRE_FL].pwm = 0; - mtc=false; - break; - } - } + break; } - break; - case 23: - if(YELLOW) { - switch(g[4]) { - case 98: - motor[TIRE_FR].pwm = fast-5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast-5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast-5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast-5; - motor[TIRE_BL].dir = BACK; - break; - case 99: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - break; - case 0: - motor[TIRE_FR].pwm = fast-5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast-5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast-5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast-5; - motor[TIRE_BL].dir = BACK; - break; - case 255: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - break; - case 253: - motor[TIRE_FR].pwm = slow+5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast-5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast-5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = slow+5; - motor[TIRE_BL].dir = BACK; - break; - case 254: - motor[TIRE_FR].pwm = normal+5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast-5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast-5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = normal+5; - motor[TIRE_BL].dir = BACK; - break; - case 1: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - break; - case 3: - motor[TIRE_FR].pwm = fast-5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = slow+5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast-5; - motor[TIRE_BL].dir = BACK; - break; - case 2: - motor[TIRE_FR].pwm = fast-5; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = normal+5; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+5; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast-5; - motor[TIRE_BL].dir = BACK; - break; - } - if(g[1]==98) { - fast=20; - normal=15; - slow=10; - } - if(g[3]==254&&countss>=100000) { - mode=24; - solenoid.solenoid4 = SOLENOID_OFF; - countss=0; - fast=60; - normal=40; - slow=20; - } - } else { - if(UPLS||DOLS) { - if(UPLS||DOLS) { - motor[MOTOR_0].pwm = 200; - motor[MOTOR_0].dir = BRAKE; - fast=60; - normal=40; - slow=25; - countss=0; - mode=24; - } - } else { - motor[MOTOR_0].pwm = 100; - motor[MOTOR_0].dir = BACK; - } - motor[TIRE_FR].pwm = 100; - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].pwm = 100; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].pwm = 100; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].pwm = 100; - motor[TIRE_BL].dir = BRAKE; + if(g[3]==0&&g[2]==0) { + mode=25; } - 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 = 25; - motor[TIRE_BL].dir = FREE; - break; - case 3: - motor[TIRE_FL].pwm = 40; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 25; - motor[TIRE_BL].dir = FREE; - break; - case 2: - motor[TIRE_FL].pwm = 30; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].pwm = 30; - motor[TIRE_BL].dir = FOR; - break; - } - if(g[3]==0&&g[2]==0) { - mode=25; - } - } else { - if(g[1]==98&&countss>=40000) { - mode=31; - countss=0; - } - switch(g[1]) { - case 98: - fast=40; - normal=30; - slow=25; - motor[TIRE_FR].pwm = normal; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = normal; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = normal; - motor[TIRE_BL].dir = BACK; - break; - case 0: - motor[TIRE_FR].pwm = fast; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 255: - motor[TIRE_FR].pwm = fast; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = slow; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 253: - motor[TIRE_FR].pwm = slow; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = slow; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 254: - motor[TIRE_FR].pwm = normal; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = normal; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 1: - motor[TIRE_FR].pwm = slow; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = fast; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = slow; - motor[TIRE_BL].dir = BACK; - mtc=true; - break; - case 3: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; - break; - case 2: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc=true; - break; - } - if(g[1]!=98&&mtc==true) { - switch(g[0]) { - case 0: - mtc=false; - break; - case 255: - motor[TIRE_FL].pwm += 3; - mtc=false; - break; - case 253: - motor[TIRE_FL].pwm += 6; - mtc=false; - break; - case 254: - motor[TIRE_FL].pwm += 9; - motor[TIRE_FR].pwm = 0; - mtc=false; - break; - case 1: - motor[TIRE_FR].pwm += 3; - mtc=false; - break; - case 3: - motor[TIRE_FR].pwm += 6; - mtc=false; - break; - case 2: - motor[TIRE_FR].pwm += 9; - motor[TIRE_FL].pwm = 0; - mtc=false; - break; - } - } - } - break; - case 25: + } else if(mode==25) { motor[TIRE_FR].pwm = 100; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].pwm = 100; @@ -2965,8 +2648,7 @@ motor[MOTOR_0].pwm = 100; motor[MOTOR_0].dir = BACK; } - break; - case 26: + } else if(mode==26) { switch(g[4]) { case 98: motor[TIRE_FR].pwm = 60; @@ -3062,227 +2744,58 @@ if(g[1]==98) { mode=31; } - break; - case 31: - motor[TIRE_FR].pwm = 255; - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].pwm = 45; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = 45; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 255; - motor[TIRE_BL].dir = BRAKE; - if(g[2]==0) { - cross=0; - countss=0; - mode=40; - } - break; - case 40: - switch(g[3]) { - case 98: - motor[TIRE_FR].pwm = normal+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = normal+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = normal+15; - motor[TIRE_BL].dir = FOR; - if(countss>30000) { - cross++; - countss=0; - } - mtc2=true; - break; - case 0: - motor[TIRE_FR].pwm = fast+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast+15; - motor[TIRE_BL].dir = FOR; - mtc2=true; - break; - case 255: - motor[TIRE_FR].pwm = slow+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = fast+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = slow+15; - motor[TIRE_BL].dir = FOR; - mtc2=true; - break; - case 253: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = slow+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc2=true; - break; - case 254: - motor[TIRE_FR].pwm = 0; - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].pwm = normal+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 0; - motor[TIRE_BL].dir = FREE; - mtc2=true; - break; - case 1: - motor[TIRE_FR].pwm = fast+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = slow+15; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+15; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = fast+15; - motor[TIRE_BL].dir = FOR; - mtc2=true; - break; - case 3: - motor[TIRE_FR].pwm = slow+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = slow+15; - motor[TIRE_BL].dir = FOR; - mtc2=true; - break; - case 2: - motor[TIRE_FR].pwm = normal+15; - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = normal+15; - motor[TIRE_BL].dir = FOR; - mtc2=true; - break; - } - if(g[2]!=98&&mtc2==true) { - switch(g[2]) { - case 0: - mtc2=false; - break; - case 255: - motor[TIRE_FR].pwm += 3; - mtc2=false; - break; - case 253: - motor[TIRE_FR].pwm += 6; - mtc2=false; - break; - case 254: - motor[TIRE_FR].pwm += 9; - motor[TIRE_BR].pwm = 0; - mtc2=false; - break; - case 1: - motor[TIRE_BR].pwm += 3; - mtc2=false; - break; - case 3: - motor[TIRE_BR].pwm += 6; - mtc2=false; - break; - case 2: - motor[TIRE_BR].pwm += 9; - motor[TIRE_FR].pwm = 0; - mtc2=false; - break; - } - } - - if(YELLOW) { - if(cross==1) { - mode=41; - countss=0; - cross=0; - } - } else { - if(cross==3) { - mode=41; - countss=0; - cross=0; - } - } - if(LIF||LIB) { - mode=42; - } - break; - case 41: - if(LIF||LIB) { - mode=42; - } - motor[TIRE_FR].pwm = 255; - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].pwm = 40; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = 40; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 255; - motor[TIRE_BL].dir = BRAKE; - if(g[1]==0) { - countss=0; - mode=70; - normal=80; - fast=100; - slow=60; - } - break; - case 42: - motor[TIRE_FR].pwm = 40; + } + } else { + if(mode==21) { + motor[TIRE_FR].pwm = 30; motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 0; - motor[TIRE_FL].dir = FREE; - motor[TIRE_BR].pwm = 0; - motor[TIRE_BR].dir = FREE; - motor[TIRE_BL].pwm = 40; + 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) { - countss=0; - mode=70; + mode=22; + fast=60; + normal=40; + slow=25; } - break; - case 70: + } else if(mode==22) { + if(g[1]==98) { + slow=25; + normal=30; + fast=40; + } + if(g[3]==0) { + slow=25; + normal=40; + fast=60; + mode=23; + } switch(g[1]) { - case 99: - if(countss>=100000) { - mode=100; - } - break; case 98: + solenoid.solenoid4 = SOLENOID_OFF; + fast=40; + normal=30; + slow=25; motor[TIRE_FR].pwm = normal; motor[TIRE_FR].dir = FOR; motor[TIRE_FL].pwm = normal; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+5; + motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = normal; motor[TIRE_BL].dir = BACK; break; case 0: - motor[TIRE_FR].pwm = 100; + motor[TIRE_FR].pwm = fast; motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 100; + motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = 105; + motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 100; + motor[TIRE_BL].pwm = fast; motor[TIRE_BL].dir = BACK; mtc=true; break; @@ -3291,7 +2804,7 @@ motor[TIRE_FR].dir = FOR; motor[TIRE_FL].pwm = slow; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+5; + motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = fast; motor[TIRE_BL].dir = BACK; @@ -3324,7 +2837,7 @@ motor[TIRE_FR].dir = FOR; motor[TIRE_FL].pwm = fast; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = fast+5; + motor[TIRE_BR].pwm = fast; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = slow; motor[TIRE_BL].dir = BACK; @@ -3335,7 +2848,7 @@ motor[TIRE_FR].dir = FREE; motor[TIRE_FL].pwm = slow; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = slow+5; + motor[TIRE_BR].pwm = slow; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = 0; motor[TIRE_BL].dir = FREE; @@ -3346,7 +2859,160 @@ motor[TIRE_FR].dir = FREE; motor[TIRE_FL].pwm = normal; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = normal+5; + motor[TIRE_BR].pwm = normal; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + } + if(g[1]!=98&&mtc==true) { + switch(g[0]) { + case 0: + mtc=false; + break; + case 255: + motor[TIRE_FL].pwm += 3; + mtc=false; + break; + case 253: + motor[TIRE_FL].pwm += 6; + mtc=false; + break; + case 254: + motor[TIRE_FL].pwm += 9; + motor[TIRE_FR].pwm = 0; + mtc=false; + break; + case 1: + motor[TIRE_FR].pwm += 3; + mtc=false; + break; + case 3: + motor[TIRE_FR].pwm += 6; + mtc=false; + break; + case 2: + motor[TIRE_FR].pwm += 9; + motor[TIRE_FL].pwm = 0; + mtc=false; + break; + } + } + } else if(mode==23) { + if(UPLS||DOLS) { + if(UPLS||DOLS) { + motor[MOTOR_0].pwm = 200; + motor[MOTOR_0].dir = BRAKE; + fast=60; + normal=40; + slow=25; + countss=0; + mode=24; + } + } else { + motor[MOTOR_0].pwm = 100; + motor[MOTOR_0].dir = BACK; + } + motor[TIRE_FR].pwm = 100; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 100; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].pwm = 100; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].pwm = 100; + motor[TIRE_BL].dir = BRAKE; + } else if(mode==24) { + if(g[1]==98&&countss>=40000) { + mode=31; + countss=0; + } + switch(g[1]) { + case 98: + fast=40; + normal=30; + slow=25; + motor[TIRE_FR].pwm = normal; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = normal; + motor[TIRE_BL].dir = BACK; + break; + case 0: + motor[TIRE_FR].pwm = fast; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 255: + motor[TIRE_FR].pwm = fast; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 253: + motor[TIRE_FR].pwm = slow; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = slow; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 254: + motor[TIRE_FR].pwm = normal; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = normal; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 1: + motor[TIRE_FR].pwm = slow; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = slow; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 3: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + case 2: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].pwm = 0; motor[TIRE_BL].dir = FREE; @@ -3386,27 +3052,367 @@ break; } } - break; - case 100: - motor[TIRE_FR].pwm = 50; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].pwm = 55; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].pwm = 60; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].pwm = 50; - motor[TIRE_BL].dir = BACK; - break; - default: - motor[TIRE_FR].pwm = 50; - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].pwm = 50; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].pwm = 50; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].pwm = 50; - motor[TIRE_BL].dir = BRAKE; - break; + } + } + + /* + if(mode==31) { + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + if(countss>=5000) { + countss=0; + mode=32; + } + } else if(mode==32) { + motor[TIRE_FR].pwm = 10; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 10; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].pwm = 10; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].pwm = 10; + motor[TIRE_BL].dir = BRAKE; + if(countss>=40000) { + mode=33; + countss=0; + } + } + */ + if(mode==31) { + motor[TIRE_FR].pwm = 255; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 45; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 45; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 255; + motor[TIRE_BL].dir = BRAKE; + if(g[2]==0) { + cross=0; + countss=0; + mode=40; + } + } else if(mode==40) { + switch(g[3]) { + case 98: + motor[TIRE_FR].pwm = normal+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = normal+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = normal+15; + motor[TIRE_BL].dir = FOR; + if(countss>30000) { + cross++; + countss=0; + } + mtc2=true; + break; + case 0: + motor[TIRE_FR].pwm = fast+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = fast+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast+15; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 255: + motor[TIRE_FR].pwm = slow+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = fast+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = slow+15; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 253: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = slow+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc2=true; + break; + case 254: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = normal+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc2=true; + break; + case 1: + motor[TIRE_FR].pwm = fast+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = slow+15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow+15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast+15; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 3: + motor[TIRE_FR].pwm = slow+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = slow+15; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 2: + motor[TIRE_FR].pwm = normal+15; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = normal+15; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + } + if(g[2]!=98&&mtc2==true) { + switch(g[2]) { + case 0: + mtc2=false; + break; + case 255: + motor[TIRE_FR].pwm += 3; + mtc2=false; + break; + case 253: + motor[TIRE_FR].pwm += 6; + mtc2=false; + break; + case 254: + motor[TIRE_FR].pwm += 9; + motor[TIRE_BR].pwm = 0; + mtc2=false; + break; + case 1: + motor[TIRE_BR].pwm += 3; + mtc2=false; + break; + case 3: + motor[TIRE_BR].pwm += 6; + mtc2=false; + break; + case 2: + motor[TIRE_BR].pwm += 9; + motor[TIRE_FR].pwm = 0; + mtc2=false; + break; + } + } + + if(YELLOW) { + if(cross==1) { + mode=41; + countss=0; + cross=0; + } + } else { + if(cross==3) { + mode=41; + countss=0; + cross=0; + } + } + if(LIF||LIB) { + mode=42; + } + + } else if(mode==41) { + if(LIF||LIB) { + mode=42; + } + motor[TIRE_FR].pwm = 255; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 40; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 255; + motor[TIRE_BL].dir = BRAKE; + if(g[1]==0) { + countss=0; + mode=70; + normal=80; + fast=100; + slow=60; + } + } else if(mode==42) { + motor[TIRE_FR].pwm = 40; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = 40; + motor[TIRE_BL].dir = BACK; + if(g[1]==0) { + countss=0; + mode=70; + } + } else if(mode==70) { + switch(g[1]) { + case 99: + if(countss>=100000) { + mode=100; + } + break; + case 98: + motor[TIRE_FR].pwm = normal; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = normal; + motor[TIRE_BL].dir = BACK; + break; + case 0: + motor[TIRE_FR].pwm = 100; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 100; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 105; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 100; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 255: + motor[TIRE_FR].pwm = fast; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = fast; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 253: + motor[TIRE_FR].pwm = slow; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = slow; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 254: + motor[TIRE_FR].pwm = normal; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = normal; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 1: + motor[TIRE_FR].pwm = slow; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = fast; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = fast+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = slow; + motor[TIRE_BL].dir = BACK; + mtc=true; + break; + case 3: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = slow; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = slow+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + case 2: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].pwm = normal; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = normal+5; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = FREE; + mtc=true; + break; + } + if(g[1]!=98&&mtc==true) { + switch(g[0]) { + case 0: + mtc=false; + break; + case 255: + motor[TIRE_FL].pwm += 3; + mtc=false; + break; + case 253: + motor[TIRE_FL].pwm += 6; + mtc=false; + break; + case 254: + motor[TIRE_FL].pwm += 9; + motor[TIRE_FR].pwm = 0; + mtc=false; + break; + case 1: + motor[TIRE_FR].pwm += 3; + mtc=false; + break; + case 3: + motor[TIRE_FR].pwm += 6; + mtc=false; + break; + case 2: + motor[TIRE_FR].pwm += 9; + motor[TIRE_FL].pwm = 0; + mtc=false; + break; + } + } + } else if(mode==100) { + motor[TIRE_FR].pwm = 50; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 55; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 60; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BL].dir = BACK; } }