大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 38:40e7fdf3c694
- Parent:
- 37:e77816810627
- Child:
- 39:784552c26163
--- a/System/Process/Process.cpp Wed Oct 30 07:32:56 2019 +0000
+++ b/System/Process/Process.cpp Sun Nov 03 05:56:53 2019 +0000
@@ -93,6 +93,7 @@
int g[8];
int Twsh;
+int testmode =0;
int mode =0;
int cross=0;//十字数える用
int cros=0;
@@ -276,7 +277,7 @@
if(ZoneSW) {
countss=0;
mode=1;
- current = 3;
+ current = 3;//10GATU30変更した
} else {
countss = 0;
@@ -293,7 +294,7 @@
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
} else {
- motor[MOTOR_0].pwm = 50;
+ motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BACK;
}
}
@@ -303,7 +304,7 @@
}
if(zyouge) {
if(Twsh==1) {
- motor[MOTOR_0].pwm = 215;
+ motor[MOTOR_0].pwm = 255;
motor[MOTOR_0].dir = FOR;
if(UPLS) {
motor[MOTOR_0].pwm = 100;
@@ -350,12 +351,12 @@
#ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag){
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
} else
-
+
#endif
{
@@ -458,10 +459,10 @@
if(controller->Button.B) {
if(dz4==true) {
if(dz4i==false) {
- solenoid.solenoid1 = SOLENOID_ON;
+ solenoid.solenoid4 = SOLENOID_ON;
dz4i=true;
} else {
- solenoid.solenoid1 = SOLENOID_OFF;
+ solenoid.solenoid4 = SOLENOID_OFF;
dz4i=false;
}
dz4=false;
@@ -1361,19 +1362,6 @@
countss=100000;
}
} else if(mode==10) { //縦ライントレース
- if(counts==false&&g[2]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[2]==99) {
- counts=false;
- }
-
- if(cros==Twsh) {
- mode=11;
- countss=0;
- cros=0;
- }//ゆっくりモードに入れなかった時の保険
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1389,7 +1377,7 @@
cross++;
countss=0;
}
- if(cross==Twsh) {
+ if(cross==3) {
mode=11;
countss=0;
cross=0;
@@ -1521,29 +1509,7 @@
mode=20;
}
} else if(mode==20) {
-
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_ON;
- }
- if(counts==false&&g[0]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[0]==99) {
- counts=false;
- }
-
- if(cros==2) {
-
- // mode=21;
- // countss=0;
- // cros=0;
- }//ゆっくりモードに入れなかった時の保険
- if(g[0]==0) {
- fast=35;
- normal=20;
- slow=15;
- }
+ solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1559,14 +1525,24 @@
countss=0;
}
mtc2=true;
- if(cross==2) {
- mode=22;
- countss=0;
- cross=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;
@@ -1679,48 +1655,9 @@
break;
}
}
- } else if(mode==21) {
- 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[2]==98&&countss>=10000) {
- mode=22;
- countss=0;
- }
}
- 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;
@@ -1729,95 +1666,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;
- countss=0;
+ } else if(mode==23) {
+ switch(g[4]) {
+ case 98:
+ 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-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;
+ 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-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;
+ 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-5;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = normal;
+ 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 = 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;
+ 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-5;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = fast;
+ 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 = fast-5;
+ motor[TIRE_BL].dir = BACK;
+ break;
}
- } 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>=100) {
+ if(DOLS==true||UPLS==true) {
+ motor[MOTOR_0].pwm = 200;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ if(g[1]==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[3]==254&&countss>=100000) {
+ mode=24;
+ solenoid.solenoid4 = SOLENOID_OFF;
countss=0;
- mode=30;
+ fast=60;
+ normal=40;
+ slow=30;
}
} else if(mode==24) {
- 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(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;
+ }
+ 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) {
+ 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 = 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 = 55;
+ 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 = 55;
+ 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 = 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 = 15;
+ 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 = 35;
+ 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 = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 55;
+ 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 = 55;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ }
+ if(g[1]==98) {
+ mode=31;
}
}
} else {
- if(mode==22) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 25;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 25;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(g[0]==0&&countss>=10000) {
- mode=30;
+ if(mode==21) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
+ mode=22;
+ fast=60;
+ normal=40;
+ slow=20;
+ }
+ } else if(mode==22) {
+ if(g[3]==0) {
+ mode=23;
+ }
+ switch(g[1]) {
+ case 98:
+ solenoid.solenoid4 = SOLENOID_OFF;
+ fast=30;
+ normal=20;
+ slow=20;
+ 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 += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].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[1]==98&&countss>=40000) {
+ mode=31;
+ countss=0;
+ }
+ switch(g[1]) {
+ case 98:
+ fast=30;
+ normal=20;
+ slow=20;
+ 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 += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
+ mtc=false;
+ break;
+ }
}
}
}
- if(mode==30) {
- motor[TIRE_FR].pwm = 59;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 62;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 59;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 62;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==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;
@@ -1845,38 +2452,19 @@
}
} else if(mode==33) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FOR;
- if(g[3]==0&&countss>=20000) {
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 00;
+ motor[TIRE_BL].dir = FREE;
+ 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;
- }
- if(counts==false&&g[0]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[0]==99) {
- counts=false;
- }
-
- if(cros==1) {
- // mode=41;
- //countss=0;
- //cros=0;
- }//ゆっくりモードに入れなかった時の保険
-
switch(g[3]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1887,12 +2475,12 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(countss>20000) {
+ if(countss>10000) {
cross++;
countss=0;
}
mtc2=true;
- if(cross==2) {
+ if(cross==1) {
mode=41;
countss=0;
cross=0;
@@ -2170,13 +2758,13 @@
}
}
} else if(mode==100) {
- motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 23;
+ motor[TIRE_BR].pwm = 43;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
} else {
}
@@ -2187,6 +2775,99 @@
#if USE_PROCESS_NUM>4
static void Process4()
{
+//中間トレーステスト
+ 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 = 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;
+ 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 = 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;
+ 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;
+ }
}
#endif
@@ -2194,13 +2875,544 @@
#if USE_PROCESS_NUM>5
static void Process5()
{
-
+ 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;
+ }
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
+//修正して中間トレーステスト
+ if(testmode==0) {
+ 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) {
+ testmode=1;
+ }
+ } else if(testmode==1) {
+ 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 = 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;
+ 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 = 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;
+ 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;
+ }
+ if(g[1]==98) {
+ testmode=2;
+ }
+ } else if(testmode==2) {
+ 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) {
+ testmode=3;
+ }
+ } else if(testmode==3) {
+ 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 = 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;
+ 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 = 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;
+ 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;
+ }
+ if(g[1]==98) {
+ testmode=4;
+ }
+ } else if(testmode==4) {
+ 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;
+ }
}
#endif