大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 39:784552c26163
- Parent:
- 38:40e7fdf3c694
- Child:
- 40:ed2409e0ce6b
--- a/System/Process/Process.cpp Sun Nov 03 05:56:53 2019 +0000
+++ b/System/Process/Process.cpp Tue Nov 05 08:38:32 2019 +0000
@@ -216,11 +216,13 @@
}
+int point1;
void SystemProcess()
{
SystemProcessInitialize();
while(1) {
+ //pc.printf("4:%d\n\r",g[4]);
if(EMC==0) {
fast=40;
normal=30;
@@ -277,7 +279,7 @@
if(ZoneSW) {
countss=0;
mode=1;
- current = 3;//10GATU30変更した
+ current = 4;//10GATU30変更した
} else {
countss = 0;
@@ -477,21 +479,20 @@
static void Process2()
{
//Oh So 青ゾーン
+ LED::LED::TapeledMode(2);
countss++;
- LED::LED::TapeledMode(4);
if(mode==1) {//スタートゾーンから白線検知
fast=40;
normal=30;
slow=20;
- motor[TIRE_FR].pwm = 27;
+ motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 33;
+ motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 33;
+ motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 27;
+ motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- //スタート後どれくらい無検知でうごくか↓
if(g[2]==0&&countss>=100000) {
countss=100000;
cross=0;
@@ -511,20 +512,6 @@
countss=100000;
}
} else if(mode==10) { //縦ライントレース
-
- if(countss==false&&g[2]==0) {
- cros++;
- countss=true;
- }
- if(countss==true&&g[2]==99) {
- countss=false;
- }
-
- if(cros==Twsh) {
- mode=11;
- countss=0;
- cros=0;
- }//ゆっくりモードに入れなかった時の保険
switch(g[1]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -536,11 +523,11 @@
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
- if(countss>100000) {
+ if(countss>80000) {
cross++;
countss=0;
}
- if(cross==Twsh) {
+ if(cross==3) {
mode=11;
countss=0;
cross=0;
@@ -672,29 +659,7 @@
mode=20;
}
} else if(mode==20) {
-
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_ON;
- }
-
- if(counts==false&&g[1]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[1]==99) {
- counts=false;
- }
-
- if(cros==2) {
- //mode=21;
- //countss=0;
- //cros=0;
- }//ゆっくりモードに入れなかった時の保険
- if(g[1]==0) {
- fast=35;
- normal=20;
- slow=15;
- }
+ solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -705,19 +670,29 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- if(countss>50000) {
+ if(countss>30000) {
cross++;
countss=0;
}
mtc2=true;
- if(cross==2) {
- mode=22;
- cross=0;
- countss=0;
- fast=40;
- normal=30;
+ if(cross==1) {
+ fast=30;
+ normal=20;
slow=20;
}
+ if(YELLOW) {
+ if(cross==2) {
+ mode=21;
+ countss=0;
+ cross=0;
+ }
+ } else {
+ if(cross==1) {
+ mode=21;
+ countss=0;
+ cross=0;
+ }
+ }
break;
case 0:
motor[TIRE_FR].pwm = fast;
@@ -830,48 +805,9 @@
break;
}
}
-
- } else if(mode==21) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- if(countss<=100000) {
- mode=22;
- }
}
- kiri=true;
- if(kiri) {
- if(mode==22) {
- 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(countss>=10000) {
- mode=23;
- }
- } else if(mode==23) {
- 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[2]==98) {
- mode=24;
- countss=0;
- }
- } else if(mode==24) {
+ if(YELLOW) {
+ if(mode==21) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
@@ -880,95 +816,765 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
- if(g[2]!=98) {
+ if(g[3]==98&&countss>=10000) {
+ mode=22;
countss=0;
- mode=25;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else if(mode==22) {
+ if(g[4]==3||g[4]==2) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ }
+ if(g[4]==253||g[4]==254) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
}
- } else if(mode==25) {
- 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>=10000) {
- mode=26;
+ switch(g[2]) {
+ case 0:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FREE;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ break;
+ }
+ switch(g[3]) {
+ case 0:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 255:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ case 253:
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 254:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 1:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 3:
+ motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FREE;
+ break;
+ case 2:
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ }
+ if(g[3]==0&&g[2]==0) {
+ mode=23;
countss=0;
}
- } else if(mode==26) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = BACK;
- if(countss>=5000) {
- mode=27;
+ } else if(mode==23) {
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ }
+ if(DOLS==true||UPLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ if(g[0]==98) {
+ fast=30;
+ normal=20;
+ slow=20;
+ if(DOLS==true||UPLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ }
+ if(g[2]==254&&countss>=100000) {
+ mode=24;
+ solenoid.solenoid4 = SOLENOID_OFF;
countss=0;
+ fast=60;
+ normal=40;
+ slow=30;
+ }
+ } else if(mode==24) {
+ if(DOLS==true||UPLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ 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;
}
- } else if(mode==27) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FOR;
- //どれくらい回転するかって値
- if(countss>=4000) {
- countss=0;
- mode=28;
+ 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;
}
- } else if(mode==28) {
- 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>=10000) {
- mode=30;
- countss=0;
+ if(g[3]==0&&g[2]==0) {
+ if(UPLS==true||DOLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ mode=25;
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ }
+ }
+ } else if(mode==25) {
+ switch(g[4]) {
+ case 98:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 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;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal-5;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ }
+ if(g[1]==98) {
+ mode=31;
}
}
} else {
- if(mode==22) {
+ if(mode==21) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(g[0]==0&&countss>=10000) {
- mode=30;
+ if(g[1]==0) {
+ mode=22;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else if(mode==22) {
+ if(g[3]==0) {
+ mode=23;
+ }
+ switch(g[0]) {
+ case 98:
+ solenoid.solenoid4 = SOLENOID_OFF;
+ fast=30;
+ normal=20;
+ slow=20;
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ }
+ if(g[0]!=98&&mtc==true) {
+ switch(g[0]) {
+ case 0:
+ mtc=false;
+ break;
+ case 255:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
+ }
+ } else if(mode==23) {
+ if(UPLS==true||DOLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ fast=60;
+ normal=40;
+ slow=20;
countss=0;
- cros=0;
+ mode=24;
+ } else {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BACK;
+ motor[TIRE_FR].pwm = 100;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 100;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 100;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 100;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+
+ } else if(mode==24) {
+ if(g[0]==98&&countss>=40000) {
+ mode=31;
+ countss=0;
+ }
+ switch(g[0]) {
+ case 98:
+ fast=30;
+ normal=20;
+ slow=20;
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ }
+ if(g[1]!=98&&mtc==true) {
+ switch(g[1]) {
+ case 0:
+ mtc=false;
+ break;
+ case 255:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
}
}
}
- if(mode==30) {
- 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 = 57;
- motor[TIRE_BL].dir = BACK;
- if(g[1]==98) {
- mode=31;
- countss=0;
- }
- } else if(mode==31) {
+ if(mode==31) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
@@ -978,42 +1584,24 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(countss>=5000) {
- mode=32;
countss=0;
- }
- } 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>=30000) {
mode=33;
- countss=0;
}
} else if(mode==33) {
motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
- if(g[3]==0&&countss>=20000) {
+ motor[TIRE_BL].dir = FOR;
+ if(g[3]==0) {
+ cross=0;
countss=0;
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_OFF;
- }
mode=40;
}
} else if(mode==40) {
- if(LIB) {
- mode=42;
- }
switch(g[3]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1024,12 +1612,12 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(countss>=10000) {
+ if(countss>10000) {
cross++;
countss=0;
}
mtc2=true;
- if(cross==2) {
+ if(cross==1) {
mode=41;
countss=0;
cross=0;
@@ -1119,29 +1707,29 @@
mtc2=false;
break;
case 255:
- motor[TIRE_FR].pwm += 5;
+ motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm += 10;
+ motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_BR].pwm = 0;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
case 1:
- motor[TIRE_BR].pwm += 5;
+ motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
case 3:
- motor[TIRE_BR].pwm += 10;
+ motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
case 2:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
}
@@ -1166,29 +1754,22 @@
slow=40;
}
} else if(mode==42) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(g[0]==0) {
+ motor[TIRE_FR].pwm = 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 = 20;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
countss=0;
- slow=40;
+ mode=70;
}
-
- } else if(mode==70)
+ } else if(mode==70) {
switch(g[0]) {
case 99:
if(countss>=100000) {
- cross++;
- mode=100;
- countss=0;
- }
- if(cross==2) {
- cross=0;
mode=100;
}
break;
@@ -1203,13 +1784,13 @@
motor[TIRE_BL].dir = FOR;
break;
case 0:
- motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].pwm = 80;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = 80;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].pwm = 80;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].pwm = 80;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
@@ -1280,48 +1861,51 @@
mtc=true;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[1]) {
- case 0:
- mtc=false;
- break;
- case 255:
- motor[TIRE_BR].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_BR].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_BR].pwm = 0;
- mtc=false;
- break;
+ if(g[1]!=98&&mtc==true) {
+ switch(g[1]) {
+ case 0:
+ mtc=false;
+ break;
+ case 255:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ }
}
} else if(mode==100) {
- motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 23;
+ motor[TIRE_BR].pwm = 43;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = FOR;
+ } else {
}
+
}
#endif
@@ -2778,14 +3362,14 @@
//中間トレーステスト
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;
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
break;
case 99:
motor[TIRE_FR].pwm = 0;
@@ -2798,14 +3382,14 @@
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;
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
break;
case 255:
motor[TIRE_FR].pwm = 0;
@@ -2818,24 +3402,24 @@
motor[TIRE_BL].dir = FREE;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
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;
+ motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = fast-5;
+ motor[TIRE_BL].dir = FOR;
break;
case 1:
motor[TIRE_FR].pwm = 0;
@@ -2848,24 +3432,24 @@
motor[TIRE_BL].dir = FREE;
break;
case 3:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 60;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = slow-5;
+ motor[TIRE_BL].dir = FOR;
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;
+ motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = normal-5;
+ motor[TIRE_BL].dir = FOR;
break;
}