大季 矢花
aa
Revision 60:24ebb611f26e, committed 2019-12-12
- Comitter:
- yabahiro
- Date:
- Thu Dec 12 07:16:20 2019 +0000
- Parent:
- 59:5cd3875fa96a
- Commit message:
- aa
Changed in this revision
| System/Process/Process.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/System/Process/Process.cpp Sat Nov 23 14:28:20 2019 +0000
+++ b/System/Process/Process.cpp Thu Dec 12 07:16:20 2019 +0000
@@ -281,7 +281,7 @@
countss=0;
mode=1;
aime.start();
- current = 3;//1119変更した
+ current = 6;//1124変更した
} else {
countss = 0;
@@ -775,11 +775,11 @@
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;
+ 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;
@@ -2517,7 +2517,7 @@
slow = 30;
}
} else {
- if(value>=2200) {
+ if(value>=2400) {
slow=25;
normal=30;
fast=40;
@@ -2528,13 +2528,16 @@
normal=30;
fast=40;
}
+
*/
- if(g[3]==0&&value>=2200) {
+ if(value>=3000) {
slow=25;
normal=40;
fast=60;
mode=23;
- }
+ value=0;
+ aime.reset();
+ }
switch(g[1]) {
case 98:
solenoid.solenoid4 = SOLENOID_OFF;
@@ -2757,12 +2760,12 @@
motor[TIRE_BL].dir = BACK;
break;
}
- if(value>=2400) {
+ if(value>=1800) {
fast=20;
normal=15;
slow=10;
}
- if(g[3]==254&&value>=2700) {
+ if(value>=2600) {
mode=24;
solenoid.solenoid4 = SOLENOID_OFF;
aime.reset();
@@ -2815,6 +2818,7 @@
break;
case 24:
if(YELLOW) {
+ /*
value=aime.read_ms();
if(value>=4000) {
mode=25;
@@ -2929,7 +2933,10 @@
if(g[3]==0&&g[2]==0) {
mode=25;
}
+ */
+ mode=25;
} else {
+ /*
if(g[1]==98&&countss>=40000) {
mode=31;
fast = 60;
@@ -2937,6 +2944,7 @@
slow = 30;
countss=0;
}
+ */
switch(g[1]) {
case 98:
fast=40;
@@ -5304,7 +5312,7 @@
{
//Oh So 青ゾーン
value=aime.read_ms();
- LED::LED::TapeledMode(4);
+ LED::LED::TapeledMode(3);//4
countss++;
switch(mode) {
case 1:
@@ -5356,7 +5364,7 @@
motor[TIRE_BL].pwm = normal+45;
motor[TIRE_BL].dir = BACK;
mtc=true;
- if(value>=1200) {
+ if(value>=1000) {
cross++;
countss=0;
value=0;
@@ -6827,6 +6835,1549 @@
#if USE_PROCESS_NUM>7
static void Process7()
{
+ //Clearly 赤ゾーン
+ LED::LED::TapeledMode(4);//3
+ value=aime.read_ms();
+ 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&&value>=1500) {
+ 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;
+ aime.reset();
+ countss=100000;
+ fast = 80;
+ normal = 60;
+ slow = 40;
+ }
+ 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(value>=1200) {
+ cross++;
+ countss=0;
+ value=0;
+ aime.reset();
+ }
+ /*
+ 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;
+ aime.reset();
+ countss=0;
+ cross=0;
+
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast+50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast+60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+45;
+ 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+60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+45;
+ 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+45;
+ 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+45;
+ 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+60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+50;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow+45;
+ 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+60;
+ 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+60;
+ 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;
+ }
+ }
+ break;
+ case 11:
+ motor[TIRE_FR].pwm = 15;
+ 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 = 15;
+ 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;
+ case 0:
+ motor[TIRE_FR].pwm = fast+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = fast+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+20;
+ motor[TIRE_BL].dir = BACK;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = normal+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = fast+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal+20;
+ motor[TIRE_BL].dir = BACK;
+ mtc2=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = slow+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = normal+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = fast+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = slow+20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow+20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast+20;
+ motor[TIRE_BL].dir = BACK;
+ mtc2=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = slow+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = slow+20;
+ motor[TIRE_BL].dir = BACK;
+ mtc2=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = normal+20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = normal+20;
+ motor[TIRE_BL].dir = BACK;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[3]) {
+ case 0:
+ mtc2=false;
+ break;
+ case 255:
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
+ break;
+ case 253:
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
+ break;
+ case 254:
+ motor[TIRE_BL].pwm += 15;
+ motor[TIRE_FL].pwm = 0;
+ mtc2=false;
+ break;
+ case 1:
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
+ break;
+ case 2:
+ motor[TIRE_FL].pwm += 15;
+ motor[TIRE_BL].pwm = 0;
+ mtc2=false;
+ break;
+ }
+ }
+ break;
+ case 21:
+ if(YELLOW) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ if(g[3]==98&&countss>=10000) {
+ mode=22;
+ aime.reset();
+ countss=0;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
+ mode=22;
+ aime.reset();
+ fast=60;
+ normal=40;
+ slow=25;
+ }
+ }
+ break;
+ case 22:
+ if(YELLOW) {
+ if(g[4]==3||g[4]==2) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ }
+ if(g[4]==253||g[4]==254) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ }
+ switch(g[2]) {
+ case 0:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ }
+ switch(g[3]) {
+ case 0:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 255:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ case 253:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 254:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 1:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 2:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ }
+ if(g[3]==0&&g[2]==0) {
+ mode=23;
+ aime.reset();
+ countss=0;
+ fast = 60;
+ normal = 40;
+ slow = 30;
+ }
+ } else {
+ if(value>=2400) {
+ slow=25;
+ normal=30;
+ fast=40;
+ }
+ /*
+ if(g[1]==98) {
+ slow=25;
+ normal=30;
+ fast=40;
+ }
+
+ */
+ if(value>=3000) {
+ slow=25;
+ normal=40;
+ fast=60;
+ mode=23;
+ value=0;
+ aime.reset();
+ }
+ 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;
+ 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(value>=1800) {
+ fast=20;
+ normal=15;
+ slow=10;
+ }
+ if(value>=2600) {
+ mode=24;
+ solenoid.solenoid4 = SOLENOID_OFF;
+ aime.reset();
+ countss=0;
+ fast=80;
+ normal=60;
+ slow=30;
+ }
+
+ /*
+ if(value>=2500) {
+ mode=24;
+ aime.reset();
+ solenoid.solenoid4 = SOLENOID_OFF;
+ countss=0;
+ fast=80;
+ normal=60;
+ slow=30;
+ }
+ if(g[1]==98) {
+ fast=10;
+ normal=10;
+ slow=10;
+ }
+ */
+ } else {
+ if(UPLS||DOLS) {
+ if(UPLS||DOLS) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ fast=60;
+ normal=40;
+ slow=30;
+ 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) {
+ /*
+ value=aime.read_ms();
+ if(value>=4000) {
+ mode=25;
+ }
+ 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;
+ }
+ */
+ mode=25;
+ } else {
+ /*
+ if(g[1]==98&&countss>=40000) {
+ mode=31;
+ fast = 60;
+ normal = 40;
+ slow = 30;
+ 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:
+ 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(UPLS==true||DOLS==true) {
+ if(UPLS==true||DOLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ countss = 0;
+ mode=26;
+ }
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ break;
+ case 26:
+ /*
+ if (g[3] == 0&&countss>=30000) {
+ mode = 40;
+ }
+ */
+ if (g[1] != 99) {
+ mode = 31;
+ }
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 60;
+ 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 = 60;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 60;
+ 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 = 25;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 25;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 40;
+ 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 = 60;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ }
+ 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[3]==0) {
+ cross=0;
+ countss=0;
+ mode=40;
+ fast = 70;
+ normal = 50;
+ slow = 30;
+ }
+ break;
+ case 40:
+ if(YELLOW) {
+ if(cross==1) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ } else {
+ if(cross==3) {
+ fast = 60;
+ normal = 40;
+ slow = 30;
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ }
+ /*
+ if(LIF||LIB) {
+ mode=42;
+ }
+ */
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = normal+15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal+15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = normal+15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = normal+15;
+ motor[TIRE_BL].dir = FOR;
+ if(countss>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;
+ }
+ }
+ break;
+ case 41:
+ /*
+ if(LIF||LIB) {
+ mode=42;
+ }
+ */
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 255;
+ motor[TIRE_BL].dir = BRAKE;
+ if(g[1]==0) {
+ countss=0;
+ mode=70;
+ normal = 120;
+ fast = 140;
+ slow = 100;
+ }
+ break;
+ case 42:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
+ countss=0;
+ mode=70;
+ }
+ break;
+ case 70:
+ switch(g[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;
+ }
+ }
+ 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;
+ }
}
#endif