大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 48:5562a99479e5
- Parent:
- 47:a9cad8502999
- Child:
- 49:697b4e9ce884
diff -r a9cad8502999 -r 5562a99479e5 System/Process/Process.cpp
--- a/System/Process/Process.cpp Tue Nov 12 08:22:43 2019 +0000
+++ b/System/Process/Process.cpp Tue Nov 12 11:49:18 2019 +0000
@@ -510,7 +510,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal+50;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal+50;
+ motor[TIRE_BL].pwm = normal+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
if(countss>80000) {
@@ -540,7 +540,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast+50;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast+50;
+ motor[TIRE_BL].pwm = fast+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -551,7 +551,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow+50;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast+50;
+ motor[TIRE_BL].pwm = fast+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -562,7 +562,7 @@
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = slow+50;
+ motor[TIRE_BL].pwm = slow+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -573,7 +573,7 @@
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = normal+50;
+ motor[TIRE_BL].pwm = normal+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -584,7 +584,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast+50;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow+50;
+ motor[TIRE_BL].pwm = slow+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -647,10 +647,10 @@
} else if(mode==11) {
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 25;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 25;
- motor[TIRE_BR].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) {
@@ -659,18 +659,18 @@
mode=20;
}
} else if(mode==20) {
- solenoid.solenoid4= SOLENOID_ON;
+ solenoid.solenoid2= SOLENOID_ON;
switch(g[2]) {
case 98:
- motor[TIRE_FR].pwm = normal+20;
+ motor[TIRE_FR].pwm = normal-20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal+20;
+ motor[TIRE_FL].pwm = normal-20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+20;
+ motor[TIRE_BR].pwm = normal-20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+20;
+ motor[TIRE_BL].pwm = normal-20;
motor[TIRE_BL].dir = BACK;
- if(countss>30000) {
+ if(countss>10000) {
cross++;
countss=0;
}
@@ -1030,7 +1030,7 @@
}
if(g[2]==254&&countss>=100000) {
mode=24;
- solenoid.solenoid4 = SOLENOID_OFF;
+ solenoid.solenoid2 = SOLENOID_OFF;
countss=0;
fast=60;
normal=40;
@@ -1157,9 +1157,12 @@
motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BRAKE;
if(DOLS==true||UPLS==true) {
- motor[MOTOR_0].pwm = 200;
- motor[MOTOR_0].dir = BRAKE;
- mode=26;
+ 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;
@@ -1278,7 +1281,7 @@
slow=25;
}
} else if(mode==22) {
- if(g[0]==98){
+ if(g[0]==98) {
slow=25;
normal=30;
fast=40;
@@ -1291,7 +1294,7 @@
}
switch(g[0]) {
case 98:
- solenoid.solenoid4 = SOLENOID_OFF;
+ solenoid.solenoid2 = SOLENOID_OFF;
fast=40;
normal=30;
slow=25;
@@ -1417,13 +1420,15 @@
}
} else if(mode==23) {
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;
+ 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;
@@ -1568,7 +1573,7 @@
}
}
}
-
+
/*
if(mode==31) {
motor[TIRE_FR].pwm = 0;
@@ -1597,7 +1602,7 @@
countss=0;
mode=40;
}
- } else
+ } else
*/
if(mode==31) {
motor[TIRE_FR].pwm = 45;
@@ -1608,7 +1613,7 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 45;
motor[TIRE_BL].dir = FOR;
- if(g[3]==0) {
+ if(g[2]==0) {
cross=0;
countss=0;
mode=40;
@@ -3398,6 +3403,1391 @@
#if USE_PROCESS_NUM>4
static void Process4()
{
+ //Oh So 決勝青ゾーン
+ LED::LED::TapeledMode(2);
+ countss++;
+ 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+50;
+ 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+50;
+ motor[TIRE_BL].dir = BACK;
+ 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;
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast+50;
+ 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+50;
+ motor[TIRE_BL].dir = BACK;
+ mtc=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = fast+50;
+ 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+50;
+ motor[TIRE_BL].dir = BACK;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = slow+50;
+ 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+50;
+ motor[TIRE_BL].dir = BACK;
+ mtc=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = normal+50;
+ 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+50;
+ motor[TIRE_BL].dir = BACK;
+ mtc=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = slow+50;
+ 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+50;
+ 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 = 25;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FOR;
+ 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.solenoid4= SOLENOID_ON;
+ switch(g[2]) {
+ case 98:
+ motor[TIRE_FR].pwm = normal+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = normal+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal+20;
+ motor[TIRE_BL].dir = BACK;
+ if(countss>30000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+
+ if(YELLOW) {
+ if(cross==1) {
+ fast=20;
+ normal=15;
+ slow=15;
+ } else if(cross==2) {
+ mode=21;
+ countss=0;
+ cross=0;
+ }
+ } else {
+ if(cross==3) {
+ mode=21;
+ countss=0;
+ cross=0;
+ }
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 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;
+ }
+ }
+ }
+ 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 = 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 = 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[2]==254&&countss>=100000) {
+ mode=24;
+ solenoid.solenoid4 = SOLENOID_OFF;
+ countss=0;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else if(mode==24) {
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ if(g[0]==0) {
+ mode=25;
+ }
+ } 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) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ mode=26;
+ countss=0;
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ } else if(mode==26) {
+ 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;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ }
+ if(g[1]!=98&&mtc==true) {
+ switch(g[1]) {
+ case 0:
+ mtc=false;
+ break;
+ case 255:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
+ }
+ } else {
+ if(mode==21) {
+ 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[1]==0) {
+ mode=22;
+ fast=60;
+ normal=40;
+ slow=25;
+ }
+ } else if(mode==22) {
+ 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.solenoid4 = 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;
+ }
+ }
+ } else if(mode==23) {
+ 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;
+ } else if(mode==24) {
+ 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;
+ 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;
+ }
+ }
+ }
+ }
+
+ /*
+ 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=33;
+ }
+ } else if(mode==33) {
+ 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[3]==0) {
+ cross=0;
+ countss=0;
+ mode=40;
+ }
+ } 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[3]==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>10000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ if(cross==1) {
+ 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 {
+ }
}
#endif