大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 50:87f5fa1bc460
- Parent:
- 49:697b4e9ce884
- Child:
- 51:a0bdbddd08f7
--- a/System/Process/Process.cpp Wed Nov 13 09:20:49 2019 +0000
+++ b/System/Process/Process.cpp Wed Nov 13 10:00:45 2019 +0000
@@ -471,6 +471,44 @@
//Oh So 青ゾーン
LED::LED::TapeledMode(2);
countss++;
+ switch(mode) {
+ case 1:
+ break;
+ case 2:
+ break;
+ case 10:
+ break;
+ case 11:
+ break;
+ case 20:
+ break;
+ case 21:
+ break;
+ case 22:
+ break;
+ case 23:
+ break;
+ case 24:
+ break;
+ case 25:
+ break;
+ case 26:
+ break;
+ case 31:
+ break;
+ case 40:
+ break;
+ case 41:
+ break;
+ case 42:
+ break;
+ case 70:
+ break;
+ case 100:
+ break;
+ default:
+ break;
+ }
if(mode==1) {//スタートゾーンから白線検知
fast=40;
normal=30;
@@ -806,9 +844,8 @@
break;
}
}
- }
- if(YELLOW) {
- if(mode==21) {
+ } else if(mode==21) {
+ if(YELLOW) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
@@ -824,7 +861,24 @@
normal=40;
slow=20;
}
- } else if(mode==22) {
+ } else {
+ 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=22;
+ fast=60;
+ normal=40;
+ slow=25;
+ }
+ }
+ } else if(mode==22) {
+ if(YELLOW) {
if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
@@ -936,357 +990,7 @@
mode=23;
countss=0;
}
- } else if(mode==23) {
- switch(g[4]) {
- case 98:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 99:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 0:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 255:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 254:
- motor[TIRE_FR].pwm = fast-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 1:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FR].pwm = slow-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = normal-5;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast-5;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast-5;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal-5;
- motor[TIRE_BL].dir = FOR;
- break;
- }
- if(g[0]==98) {
- fast=30;
- normal=25;
- slow=20;
- }
- if(g[2]==254&&countss>=100000) {
- mode=24;
- solenoid.solenoid2 = SOLENOID_OFF;
- countss=0;
- fast=60;
- normal=40;
- slow=20;
- }
- } else if(mode==24) {
- if(g[4]==3||g[4]==2) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 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=25;
- }
- } else if(mode==25) {
- motor[TIRE_FR].pwm = 100;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 100;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 100;
- motor[TIRE_BL].dir = BRAKE;
- if(DOLS==true||UPLS==true) {
- if(DOLS==true||UPLS==true) {
- motor[MOTOR_0].pwm = 200;
- motor[MOTOR_0].dir = BRAKE;
- countss=0;
- mode=26;
- }
- } else {
- motor[MOTOR_0].pwm = 100;
- motor[MOTOR_0].dir = BACK;
- }
- } else if(mode==26) {
- switch(g[4]) {
- case 98:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 60;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 60;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 60;
- motor[TIRE_BL].dir = FOR;
- break;
- case 99:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 60;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 60;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 60;
- motor[TIRE_BL].dir = FOR;
- break;
- case 255:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 60;
- motor[TIRE_BL].dir = FOR;
- break;
- case 254:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 60;
- motor[TIRE_BL].dir = FOR;
- break;
- case 1:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 60;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 60;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 60;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 60;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = FOR;
- break;
- }
- if(g[0]==98) {
- mode=31;
- }
- }
- } else {
- if(mode==21) {
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 255;
- motor[TIRE_BL].dir = BRAKE;
- if(g[0]==0) {
- mode=22;
- fast=60;
- normal=40;
- slow=25;
- }
- } else if(mode==22) {
+ } else {
if(g[0]==98) {
slow=25;
normal=30;
@@ -1424,7 +1128,115 @@
break;
}
}
- } else if(mode==23) {
+ }
+ } else if(mode==23) {
+ if(YELLOW) {
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 99:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = fast-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = slow-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = normal-5;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast-5;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast-5;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ }
+ if(g[0]==98) {
+ fast=30;
+ normal=25;
+ slow=20;
+ }
+ if(g[2]==254&&countss>=100000) {
+ mode=24;
+ solenoid.solenoid2 = SOLENOID_OFF;
+ countss=0;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else {
if(UPLS==true||DOLS==true) {
if(UPLS==true||DOLS==true) {
motor[MOTOR_0].pwm = 200;
@@ -1447,7 +1259,120 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BRAKE;
- } else if(mode==24) {
+ }
+ } else if(mode==24) {
+ if(YELLOW) {
+ if(g[4]==3||g[4]==2) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ }
+ if(g[4]==253||g[4]==254) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ }
+ switch(g[2]) {
+ case 0:
+ motor[TIRE_FR].pwm = 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=25;
+ }
+ } else {
if(g[0]==98&&countss>=40000) {
mode=31;
countss=0;
@@ -1578,39 +1503,123 @@
}
}
}
- }
-
- /*
- if(mode==31) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(countss>=5000) {
- countss=0;
- mode=33;
+ } else if(mode==25) {
+ motor[TIRE_FR].pwm = 100;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 100;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 100;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 100;
+ motor[TIRE_BL].dir = BRAKE;
+ if(DOLS==true||UPLS==true) {
+ if(DOLS==true||UPLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ countss=0;
+ mode=26;
+ }
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
}
- } else if(mode==33) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- if(g[3]==0) {
- cross=0;
- countss=0;
- mode=40;
+ } else if(mode==26) {
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 99:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = FOR;
+ break;
}
- } else
- */
- if(mode==31) {
+ if(g[0]==98) {
+ mode=31;
+ }
+ } else if(mode==31) {
motor[TIRE_FR].pwm = 45;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 255;