大季 矢花
/
MB2019_main_11_17_home
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 54:f80ecacbde43, committed 2019-11-16
- Comitter:
- yabahiro
- Date:
- Sat Nov 16 23:59:11 2019 +0000
- Parent:
- 53:694dbd7dfa0e
- Commit message:
- aa
Changed in this revision
| Communication/RS485/RS485.cpp | Show annotated file Show diff for this revision Revisions of this file |
| System/Process/Process.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Communication/RS485/RS485.cpp Fri Nov 15 11:49:03 2019 +0000
+++ b/Communication/RS485/RS485.cpp Sat Nov 16 23:59:11 2019 +0000
@@ -14,7 +14,7 @@
Serial RS485Line(RS485LINE_TX, RS485LINE_RX);
bool readFase = 0;
- char buffer[RS485_BUFFER_LINE] = {0};
+ signed char buffer[RS485_BUFFER_LINE] = {0};
void Transmit();
void Recieve();
--- a/System/Process/Process.cpp Fri Nov 15 11:49:03 2019 +0000
+++ b/System/Process/Process.cpp Sat Nov 16 23:59:11 2019 +0000
@@ -255,7 +255,7 @@
}
}
for(int i = 0; i < 8; i++) {
- g[i] = LineHub::GetPara(i);
+ g[i]=LineHub::GetPara(i);
}
if(ThSW) {
Twsh=1;
@@ -547,7 +547,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast+55;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow+50;
@@ -558,7 +558,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow+55;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -569,7 +569,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal+55;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -580,7 +580,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow+55;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+50;
@@ -591,7 +591,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+50;
@@ -602,7 +602,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+50;
@@ -619,28 +619,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].pwm = 0;
mtc=false;
@@ -657,7 +657,7 @@
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) {
+ if(g[2]==0 || g[2]==-3 || g[2]==-4 || g[2]==-6) {
countss=0;
cross=0;
mode=20;
@@ -710,7 +710,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+20;
@@ -721,7 +721,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+20;
@@ -732,7 +732,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+20;
@@ -743,7 +743,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow+20;
@@ -754,7 +754,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -765,7 +765,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -782,28 +782,28 @@
case 0:
mtc2=false;
break;
- case 255:
+ case -3:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
- case 253:
+ case -6:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
- case 254:
+ case -4:
motor[TIRE_BL].pwm += 15;
motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
- case 1:
+ case 3:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
- case 3:
+ case 6:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
- case 2:
+ case 4:
motor[TIRE_FL].pwm += 15;
motor[TIRE_BL].pwm = 0;
mtc2=false;
@@ -857,7 +857,7 @@
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
- if(g[4]==253||g[4]==254) {
+ if(g[4]==-6||g[4]==-4) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
@@ -874,37 +874,37 @@
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
@@ -918,36 +918,36 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm = 25;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- case 253:
+ case -6:
motor[TIRE_FL].pwm = 25;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FL].pwm = 45;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 2:
+ case 4:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
@@ -996,7 +996,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow+5;
@@ -1007,7 +1007,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1018,7 +1018,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1029,7 +1029,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+5;
@@ -1040,7 +1040,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+5;
@@ -1051,7 +1051,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+5;
@@ -1068,28 +1068,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
@@ -1131,7 +1131,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -1141,7 +1141,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -1151,7 +1151,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
@@ -1161,7 +1161,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -1171,7 +1171,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -1181,7 +1181,7 @@
motor[TIRE_BL].pwm = slow-5;
motor[TIRE_BL].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -1197,7 +1197,7 @@
normal=15;
slow=10;
}
- if(g[2]==254&&countss>=100000) {
+ if(g[2]==-4&&countss>=100000) {
mode=24;
solenoid.solenoid2 = SOLENOID_OFF;
countss=0;
@@ -1242,7 +1242,7 @@
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
- if(g[4]==253||g[4]==254) {
+ if(g[4]==-6||g[4]==-4) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
@@ -1259,37 +1259,37 @@
motor[TIRE_BR].pwm = 25;
motor[TIRE_BR].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 25;
motor[TIRE_BR].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 25;
motor[TIRE_BR].dir = FREE;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 25;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 25;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
@@ -1303,36 +1303,36 @@
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm = 25;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- case 253:
+ case -6:
motor[TIRE_FL].pwm = 25;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FL].pwm = 45;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = FREE;
break;
- case 2:
+ case 4:
motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
@@ -1372,7 +1372,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow+5;
@@ -1383,7 +1383,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1394,7 +1394,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1405,7 +1405,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+5;
@@ -1416,7 +1416,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+5;
@@ -1427,7 +1427,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+5;
@@ -1444,28 +1444,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
@@ -1530,7 +1530,7 @@
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -1540,7 +1540,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
@@ -1550,7 +1550,7 @@
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 45;
@@ -1560,7 +1560,7 @@
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -1570,7 +1570,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 25;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 65;
@@ -1580,7 +1580,7 @@
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 65;
@@ -1654,7 +1654,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -1665,7 +1665,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
@@ -1676,7 +1676,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -1687,7 +1687,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -1698,7 +1698,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1709,7 +1709,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1726,28 +1726,28 @@
case 0:
mtc2=false;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
- case 253:
+ case -6:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_FL].pwm = 0;
mtc2=false;
@@ -1819,7 +1819,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -1830,7 +1830,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1841,7 +1841,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -1852,7 +1852,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -1863,7 +1863,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
@@ -1874,7 +1874,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -1891,28 +1891,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
@@ -1950,688 +1950,1010 @@
//Clearly 赤ゾーン
LED::LED::TapeledMode(3);
countss++;
- if(mode==1) {//スタートゾーンから白線検知
- fast=40;
- normal=30;
- slow=20;
- motor[TIRE_FR].pwm = normal+20;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+20;
- motor[TIRE_BL].dir = FOR;
- if(g[2]==0&&countss>=100000) {
- countss=100000;
- cross=0;
- mode=3;
- }
- } else if(mode==3) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 255;
- motor[TIRE_BL].dir = BRAKE;
- if(g[0]==0) {
- mode=10;
- countss=100000;
- }
- } else if(mode==10) { //縦ライントレース
- switch(g[0]) {
- case 98:
- motor[TIRE_FR].pwm = normal+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- if(countss>80000) {
- cross++;
- countss=0;
- }
- if(cross==2) {
- fast = 10;
- normal = 10;
- slow = 5;
- }
-
- if(cross==3) {
- fast=40;
- normal=30;
- slow=20;
- mode=11;
- countss=0;
- cross=0;
+ switch(mode) {
+ case 1:
+ fast=40;
+ normal=30;
+ slow=20;
+ motor[TIRE_FR].pwm = normal+20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal+20;
+ motor[TIRE_BL].dir = FOR;
+ if(g[2]==0&&countss>=100000) {
+ countss=100000;
+ cross=0;
+ mode=3;
+ }
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 255;
+ motor[TIRE_BL].dir = BRAKE;
+ if(g[0]==0) {
+ mode=10;
+ countss=100000;
+ }
+ break;
+ case 10:
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = normal+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal+50;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ if(countss>80000) {
+ cross++;
+ countss=0;
+ }
+ if(cross==2) {
+ fast = 10;
+ normal = 10;
+ slow = 5;
+ }
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = fast+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = fast+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = slow+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = slow+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 254:
- motor[TIRE_FR].pwm = normal+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = normal+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = slow+50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow+50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
- break;
- case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal+50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+50;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
- break;
- }
- if(g[0]!=98&&mtc==true) {
- switch(g[1]) {
- case 0:
- mtc=false;
- break;
- case 255:
- motor[TIRE_BR].pwm += 3;
- mtc=false;
- break;
- case 253:
- motor[TIRE_BR].pwm += 6;
- mtc=false;
- break;
- case 254:
- motor[TIRE_BR].pwm += 9;
- motor[TIRE_BL].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 3;
- mtc=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 6;
- mtc=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 9;
- motor[TIRE_BR].pwm = 0;
- mtc=false;
- break;
- }
- }
- } else if(mode==11) {
- motor[TIRE_FR].pwm = 25;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 25;
- motor[TIRE_BL].dir = BACK;
- if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
- countss=0;
- cross=0;
- mode=20;
- }
- } else if(mode==20) {
- solenoid.solenoid4= SOLENOID_ON;
- switch(g[2]) {
- case 98:
- motor[TIRE_FR].pwm = normal+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+20;
- motor[TIRE_BL].dir = BACK;
- if(countss>30000) {
- cross++;
- countss=0;
- }
- mtc2=true;
-
- if(YELLOW) {
- if(cross==1) {
- fast=20;
- normal=15;
- slow=15;
- } else if(cross==2) {
- mode=21;
- countss=0;
- cross=0;
- }
- } else {
if(cross==3) {
- mode=21;
+ fast=40;
+ normal=30;
+ slow=20;
+ mode=11;
countss=0;
cross=0;
+
}
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = fast+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+20;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = normal+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+20;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = fast+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow+20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+20;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 3:
- motor[TIRE_FR].pwm = slow+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = slow+20;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 2:
- motor[TIRE_FR].pwm = normal+20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = normal+20;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- }
- if(g[2]!=98&&mtc2==true) {
- switch(g[3]) {
+ break;
case 0:
- mtc2=false;
+ motor[TIRE_FR].pwm = fast+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast+50;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
break;
- case 255:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
+ case -3:
+ motor[TIRE_FR].pwm = fast+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow+50;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
break;
- case 253:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
+ case -6:
+ motor[TIRE_FR].pwm = slow+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = slow+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
break;
- case 254:
- motor[TIRE_BL].pwm += 15;
+ case -4:
+ motor[TIRE_FR].pwm = normal+50;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = normal+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
break;
case 3:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
+ motor[TIRE_FR].pwm = slow+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast+50;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow+50;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
break;
- case 2:
- motor[TIRE_FL].pwm += 15;
+ case 6:
+ 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;
- mtc2=false;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ case 4:
+ 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(YELLOW) {
- if(mode==21) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FOR;
- if(g[3]==98&&countss>=10000) {
- mode=22;
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
+ case 0:
+ mtc=false;
+ break;
+ case -3:
+ motor[TIRE_BR].pwm += 3;
+ mtc=false;
+ break;
+ case -6:
+ motor[TIRE_BR].pwm += 6;
+ mtc=false;
+ break;
+ case -4:
+ motor[TIRE_BR].pwm += 9;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 3;
+ mtc=false;
+ break;
+ case 6:
+ motor[TIRE_BL].pwm += 6;
+ mtc=false;
+ break;
+ case 4:
+ motor[TIRE_BL].pwm += 9;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
+ break;
+ case 11:
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 25;
+ motor[TIRE_BL].dir = BACK;
+ if(g[2]==0 || g[2]==-3 || g[2]==-4 || g[2]==-6) {
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;
+ cross=0;
+ mode=20;
}
- if(g[4]==253||g[4]==254) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- }
+ break;
+ case 20:
+ solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
- case 0:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
+ 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 255:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
+ 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 253:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
+ case -3:
+ 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 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 30;
+ case -6:
+ 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 1:
- motor[TIRE_FR].pwm = 20;
+ case -4:
+ motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].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 = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
break;
case 3:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- }
- switch(g[3]) {
- case 0:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 255:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 30;
+ motor[TIRE_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;
- 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;
+ mtc2=true;
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;
+ case 6:
+ 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;
- }
- 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;
+ case 4:
+ motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = BACK;
- break;
- case 99:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 0:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = BACK;
- break;
- case 255:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = slow+5;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow+5;
+ motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = BACK;
- break;
- case 254:
- motor[TIRE_FR].pwm = normal+5;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal+5;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow+5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow+5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = BACK;
- break;
- case 2:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal+5;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal+5;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = BACK;
+ mtc2=true;
break;
}
- if(g[1]==98) {
- fast=20;
- normal=15;
- slow=10;
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[3]) {
+ case 0:
+ mtc2=false;
+ break;
+ case -3:
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
+ break;
+ case -6:
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
+ break;
+ case -4:
+ motor[TIRE_BL].pwm += 15;
+ motor[TIRE_FL].pwm = 0;
+ mtc2=false;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
+ break;
+ case 6:
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
+ break;
+ case 4:
+ motor[TIRE_FL].pwm += 15;
+ motor[TIRE_BL].pwm = 0;
+ mtc2=false;
+ break;
+ }
}
- if(g[3]==254&&countss>=100000) {
- mode=24;
- solenoid.solenoid4 = SOLENOID_OFF;
- countss=0;
- fast=60;
- normal=40;
- slow=20;
- }
- } else if(mode==24) {
- if(g[4]==3||g[4]==2) {
+ break;
+ case 21:
+ if(YELLOW) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ if(g[3]==98&&countss>=10000) {
+ mode=22;
+ countss=0;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- }
- if(g[4]==253||g[4]==254) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
+ if(g[1]==0) {
+ mode=22;
+ fast=60;
+ normal=40;
+ slow=25;
+ }
}
- switch(g[2]) {
- case 0:
- motor[TIRE_FR].pwm = 25;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 25;
- motor[TIRE_BR].dir = FREE;
- break;
- case 255:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 25;
- motor[TIRE_BR].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 25;
- motor[TIRE_BR].dir = FREE;
- break;
- case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- break;
- case 1:
- motor[TIRE_FR].pwm = 25;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- case 3:
- motor[TIRE_FR].pwm = 25;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = FOR;
- break;
- case 2:
+ break;
+ case 22:
+ if(YELLOW) {
+ if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- }
- switch(g[3]) {
- case 0:
- motor[TIRE_FL].pwm = 25;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 25;
- motor[TIRE_BL].dir = FREE;
- break;
- case 255:
- motor[TIRE_FL].pwm = 25;
- motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- case 253:
- motor[TIRE_FL].pwm = 25;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 254:
+ }
+ if(g[4]==-6||g[4]==-4) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
+ motor[TIRE_BL].dir = FOR;
+ }
+ switch(g[2]) {
+ case 0:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -3:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -6:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -4:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 6:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 4:
+ 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 -3:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ case -6:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case -4:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 6:
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 4:
+ 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 -3:
+ 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 -6:
+ 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 -4:
+ 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 3:
+ 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 6:
+ 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 4:
+ 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 -3:
+ motor[TIRE_FL].pwm += 3;
+ mtc=false;
+ break;
+ case -6:
+ motor[TIRE_FL].pwm += 6;
+ mtc=false;
+ break;
+ case -4:
+ motor[TIRE_FL].pwm += 9;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm += 3;
+ mtc=false;
+ break;
+ case 6:
+ motor[TIRE_FR].pwm += 6;
+ mtc=false;
+ break;
+ case 4:
+ motor[TIRE_FR].pwm += 9;
+ motor[TIRE_FL].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
+ }
+ break;
+ case 23:
+ if(YELLOW) {
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 99:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case -3:
+ 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 -6:
+ 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 -4:
+ 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 3:
+ 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 6:
+ 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 4:
+ 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]==-4&&countss>=100000) {
+ mode=24;
+ solenoid.solenoid4 = SOLENOID_OFF;
+ countss=0;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else {
+ if(UPLS||DOLS) {
+ if(UPLS||DOLS) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ fast=60;
+ normal=40;
+ slow=25;
+ countss=0;
+ mode=24;
+ }
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ motor[TIRE_FR].pwm = 100;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 100;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 100;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 100;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+ break;
+ case 24:
+ if(YELLOW) {
+ if(g[4]==3||g[4]==2) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 25;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FL].pwm = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 25;
- motor[TIRE_BL].dir = FREE;
- break;
- case 2:
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ }
+ if(g[4]==-6||g[4]==-4) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- break;
+ }
+ switch(g[2]) {
+ case 0:
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -3:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -6:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case -4:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 6:
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 4:
+ 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 -3:
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ case -6:
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case -4:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 25;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 6:
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 25;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 4:
+ 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 -3:
+ 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 -6:
+ 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 -4:
+ 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 3:
+ 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 6:
+ 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 4:
+ 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 -3:
+ motor[TIRE_FL].pwm += 3;
+ mtc=false;
+ break;
+ case -6:
+ motor[TIRE_FL].pwm += 6;
+ mtc=false;
+ break;
+ case -4:
+ motor[TIRE_FL].pwm += 9;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm += 3;
+ mtc=false;
+ break;
+ case 6:
+ motor[TIRE_FR].pwm += 6;
+ mtc=false;
+ break;
+ case 4:
+ motor[TIRE_FR].pwm += 9;
+ motor[TIRE_FL].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
}
- if(g[3]==0&&g[2]==0) {
- mode=25;
- }
- } else if(mode==25) {
+ break;
+ case 25:
motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 100;
@@ -2648,7 +2970,8 @@
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
- } else if(mode==26) {
+ break;
+ case 26:
switch(g[4]) {
case 98:
motor[TIRE_FR].pwm = 60;
@@ -2680,7 +3003,7 @@
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -2690,7 +3013,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 25;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
@@ -2700,7 +3023,7 @@
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = BACK;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
@@ -2710,7 +3033,7 @@
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -2720,7 +3043,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 25;
@@ -2730,7 +3053,7 @@
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 40;
@@ -2744,73 +3067,242 @@
if(g[1]==98) {
mode=31;
}
- }
- } else {
- if(mode==21) {
- motor[TIRE_FR].pwm = 30;
+ break;
+ case 31:
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 45;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 45;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 255;
+ motor[TIRE_BL].dir = BRAKE;
+ if(g[2]==0) {
+ cross=0;
+ countss=0;
+ mode=40;
+ }
+ break;
+ case 40:
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = normal+15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal+15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = normal+15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = normal+15;
+ motor[TIRE_BL].dir = FOR;
+ if(countss>30000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast+15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast+15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = fast+15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = fast+15;
+ motor[TIRE_BL].dir = FOR;
+ mtc2=true;
+ break;
+ case -3:
+ 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 -6:
+ 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 -4:
+ 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 3:
+ 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 6:
+ 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 4:
+ 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 -3:
+ motor[TIRE_FR].pwm += 3;
+ mtc2=false;
+ break;
+ case -6:
+ motor[TIRE_FR].pwm += 6;
+ mtc2=false;
+ break;
+ case -4:
+ motor[TIRE_FR].pwm += 9;
+ motor[TIRE_BR].pwm = 0;
+ mtc2=false;
+ break;
+ case 3:
+ motor[TIRE_BR].pwm += 3;
+ mtc2=false;
+ break;
+ case 6:
+ motor[TIRE_BR].pwm += 6;
+ mtc2=false;
+ break;
+ case 4:
+ motor[TIRE_BR].pwm += 9;
+ motor[TIRE_FR].pwm = 0;
+ mtc2=false;
+ break;
+ }
+ }
+
+ if(YELLOW) {
+ if(cross==1) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ } else {
+ if(cross==3) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ }
+ if(LIF||LIB) {
+ mode=42;
+ }
+ break;
+ case 41:
+ if(LIF||LIB) {
+ mode=42;
+ }
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 255;
+ motor[TIRE_BL].dir = BRAKE;
+ if(g[1]==0) {
+ countss=0;
+ mode=70;
+ normal=80;
+ fast=100;
+ slow=60;
+ }
+ break;
+ case 42:
+ motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
- mode=22;
- fast=60;
- normal=40;
- slow=25;
- }
- } else if(mode==22) {
- if(g[1]==98) {
- slow=25;
- normal=30;
- fast=40;
+ countss=0;
+ mode=70;
}
- if(g[3]==0) {
- slow=25;
- normal=40;
- fast=60;
- mode=23;
- }
+ break;
+ case 70:
switch(g[1]) {
+ case 99:
+ if(countss>=100000) {
+ mode=100;
+ }
+ break;
case 98:
- solenoid.solenoid4 = SOLENOID_OFF;
- fast=40;
- normal=30;
- slow=25;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = normal+5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
break;
case 0:
+ motor[TIRE_FR].pwm = 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 -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
+ 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 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:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -2821,7 +3313,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -2832,187 +3324,34 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 1:
+ case 3:
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].pwm = fast+5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
- break;
- case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = slow+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==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:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = normal+5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -3024,397 +3363,56 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].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=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;
- }
+ 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 = 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;
- }
-
}
#endif
@@ -3501,7 +3499,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast+50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow+50;
@@ -3512,7 +3510,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow+50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -3523,7 +3521,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal+50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -3534,7 +3532,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow+50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+50;
@@ -3545,7 +3543,7 @@
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+50;
@@ -3556,7 +3554,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+50;
@@ -3573,28 +3571,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].pwm = 0;
mtc=false;
@@ -3610,7 +3608,7 @@
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) {
+ if(g[2]==0 || g[2]==-3 || g[2]==-4 || g[2]==-6) {
countss=0;
cross=0;
mode=20;
@@ -3667,7 +3665,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+20;
@@ -3678,7 +3676,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+20;
@@ -3689,7 +3687,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+20;
@@ -3700,7 +3698,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow+20;
@@ -3711,7 +3709,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -3722,7 +3720,7 @@
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
@@ -3739,28 +3737,28 @@
case 0:
mtc2=false;
break;
- case 255:
+ case -3:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
- case 253:
+ case -6:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
- case 254:
+ case -4:
motor[TIRE_BL].pwm += 15;
motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
- case 1:
+ case 3:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
- case 3:
+ case 6:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
- case 2:
+ case 4:
motor[TIRE_FL].pwm += 15;
motor[TIRE_BL].pwm = 0;
mtc2=false;
@@ -3796,7 +3794,7 @@
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
- if(g[4]==253||g[4]==254) {
+ if(g[4]==-6||g[4]==-4) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
@@ -3813,37 +3811,37 @@
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
@@ -3857,36 +3855,36 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- case 253:
+ case -6:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
- case 1:
+ case 3:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
- case 2:
+ case 4:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
@@ -3929,7 +3927,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -3939,7 +3937,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow-5;
@@ -3949,7 +3947,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal-5;
@@ -3959,7 +3957,7 @@
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -3969,7 +3967,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast-5;
@@ -3979,7 +3977,7 @@
motor[TIRE_BL].pwm = slow-5;
motor[TIRE_BL].dir = FOR;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal-5;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast-5;
@@ -3990,7 +3988,7 @@
motor[TIRE_BL].dir = FOR;
break;
}
- if(g[2]==254&&countss>=100000) {
+ if(g[2]==-4&&countss>=100000) {
mode=24;
solenoid.solenoid4 = SOLENOID_OFF;
countss=0;
@@ -4058,7 +4056,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -4069,7 +4067,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4080,7 +4078,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4091,7 +4089,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -4102,7 +4100,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
@@ -4113,7 +4111,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -4130,28 +4128,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
@@ -4193,7 +4191,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -4204,7 +4202,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4215,7 +4213,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4226,7 +4224,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -4237,7 +4235,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
@@ -4248,7 +4246,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -4265,28 +4263,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
@@ -4373,7 +4371,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = slow+15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+15;
@@ -4384,7 +4382,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+15;
@@ -4395,7 +4393,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+15;
@@ -4406,7 +4404,7 @@
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = fast+15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow+15;
@@ -4417,7 +4415,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = slow+15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4428,7 +4426,7 @@
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = normal+15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4445,28 +4443,28 @@
case 0:
mtc2=false;
break;
- case 255:
+ case -3:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
- case 253:
+ case -6:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
- case 254:
+ case -4:
motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_FL].pwm = 0;
mtc2=false;
@@ -4533,7 +4531,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 255:
+ case -3:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
@@ -4544,7 +4542,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 253:
+ case -6:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4555,7 +4553,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 254:
+ case -4:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -4566,7 +4564,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 1:
+ case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
@@ -4577,7 +4575,7 @@
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
- case 3:
+ case 6:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
@@ -4588,7 +4586,7 @@
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
- case 2:
+ case 4:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -4605,28 +4603,28 @@
case 0:
mtc=false;
break;
- case 255:
+ case -3:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
- case 253:
+ case -6:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
- case 254:
+ case -4:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
- case 1:
+ case 3:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
- case 3:
+ case 6:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
- case 2:
+ case 4:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;