大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 53:694dbd7dfa0e
- Parent:
- 52:4394a1debceb
- Child:
- 54:4167f10ecad0
--- 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;
}
}