大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Revision 31:45f97698857b, committed 2019-09-21
- Comitter:
- yabahiro
- Date:
- Sat Sep 21 04:28:31 2019 +0000
- Parent:
- 30:9450dc1d53cc
- Commit message:
- aa;
Changed in this revision
--- a/Input/Potentiometer/Potentiometer.cpp Fri Sep 20 02:47:02 2019 +0000
+++ b/Input/Potentiometer/Potentiometer.cpp Sat Sep 21 04:28:31 2019 +0000
@@ -2,12 +2,12 @@
#include "mbed.h"
namespace POTENTIOMETER {
- AnalogIn adc[] = {
- AnalogIn(ADC0_PIN),
- AnalogIn(ADC1_PIN),
- AnalogIn(ADC2_PIN),
- AnalogIn(ADC3_PIN),
- AnalogIn(ADC4_PIN),
+ DigitalOut dio[]= {
+ DigitalOut(ADC0_PIN),
+ DigitalOut(ADC1_PIN),
+ DigitalOut(ADC2_PIN),
+ DigitalOut(ADC3_PIN),
+ DigitalOut(ADC4_PIN),
};
}
--- a/Input/Potentiometer/Potentiometer.h Fri Sep 20 02:47:02 2019 +0000
+++ b/Input/Potentiometer/Potentiometer.h Sat Sep 21 04:28:31 2019 +0000
@@ -10,7 +10,7 @@
#define ADC3_PIN PC_0
#define ADC4_PIN PB_0
- extern AnalogIn adc[];
+ extern DigitalOut dio[];
}
#endif
--- a/Input/Switch/Switch.cpp Fri Sep 20 02:47:02 2019 +0000
+++ b/Input/Switch/Switch.cpp Sat Sep 21 04:28:31 2019 +0000
@@ -58,7 +58,7 @@
ch.all = 11;
break;
case 4:
- ch.all = 6;
+ ch.all = 7;
break;
case 5:
ch.all = 4;
@@ -70,7 +70,7 @@
ch.all = 0;
break;
case 8:
- ch.all = 7;
+ ch.all = 6;
break;
case 9:
ch.all = 5;
--- a/System/Process/Process.cpp Fri Sep 20 02:47:02 2019 +0000
+++ b/System/Process/Process.cpp Sat Sep 21 04:28:31 2019 +0000
@@ -104,6 +104,25 @@
else return abs(pwmVal);
}
+int Twsh;
+bool UP_flag = false;
+bool Air_flag = false;
+bool dz1=true;
+bool dz1i=false;
+bool dz2=true;
+bool dz2i=false;
+bool dz3=true;
+bool dz3i=false;
+bool dz4=true;
+bool dz4i=false;
+bool zone;
+bool through=false;
+
+int mode =1;
+int cross=0;//十字数える用
+int count=100000;//wait代替え
+int slow=30;
+
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -264,46 +283,72 @@
}
}
-
-int Twsh;
-
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
+ for(int i=0; i<=3; i++) {
+ if(i==0) {
+ POTENTIOMETER::dio[0]= 1;
+ } else {
+ POTENTIOMETER::dio[i]=0;
+ }
+ }
if(StertSW) {
- current = 2;
- Twsh=3;
+ if(ZoneSW) {
+ if(ThSW) {
+ Twsh=1;
+ } else {
+ Twsh=2;
+ }
+ mode=1;
+ current = 2;
+ } else {
+ if(ThSW) {
+ Twsh=1;
+ } else {
+ Twsh=2;
+ }
+ mode=1;
+ current = 3;
+ }
}
+
+ if(UPSW) {
+ UP_flag = true;
+ }
+
+ if(UP_flag) {
+ motor[MOTOR_0].dir = FOR;
+ motor[MOTOR_0].pwm = 100;
+
+ if (UPLS) {
+ motor[MOTOR_0].dir = BRAKE;
+ UP_flag = false;
+ Air_flag = true;
+ }
+ }
+ if(Air_flag) {
+ solenoid.solenoid2 = SOLENOID_ON;
+ }
+
}
#endif
#if USE_PROCESS_NUM>1
-
-bool dz1=true;
-bool dz1i=false;
-bool dz2=true;
-bool dz2i=false;
-bool dz3=true;
-bool dz3i=false;
-bool dz4=true;
-bool dz4i=false;
-
-bool zone;
-bool through=false;
-
-int mode =1;
-int cross=0;//十字数える用
-int count=100000;//wait代替え
-int slow=30;
-
-
static void Process1()
{
+ for(int i=0; i<=3; i++) {
+ if(i==1) {
+ POTENTIOMETER::dio[1]=1;
+ } else {
+ POTENTIOMETER::dio[i]=0;
+ }
+ }
count=100000;
- mode=0;
+ mode=1;
cross=0;
motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
@@ -414,41 +459,241 @@
static void Process2()
{
- // printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
- count++;
- if(mode==0&&StertSW) {
- if(ThSW) {
- zone=ZoneSW;
- Twsh=1;
- mode=1;
+ for(int i=0; i<=3; i++) {
+ if(i==2) {
+ POTENTIOMETER::dio[2]=1;
} else {
- zone=ZoneSW;
- Twsh=2;
- mode=1;
+ POTENTIOMETER::dio[i]=0;
}
}
- if(ZoneSW==false) {
- if(mode==1) {//スタートゾーンから白線検知
+// printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
+ count++;
+ if(mode==1) {//スタートゾーンから白線検知
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=2;
+ }
+ } else if(mode==2) {//横移動
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=3;
+ mtc2=true;
+ } else {
motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=2;
+ motor[TIRE_BL].dir = BACK;
+ }
+ } else if(mode==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;
+ if(g[0]==0) {
+ through=true;
+ }
+ if(through==true&&g[0]==255) {
+ through=false;
+ count=0;
+ mode=4;
+ }
+ } else if(mode==4) { //横ライントレースから縦ライントレースへ
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ if(g[0]==0) {
+ count=0;
+ cross=0;
+ mode=10;
+ }
+ } else if(g[0]!=99&&mode==10) { //縦ライントレース
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==Twsh) {
+ mode=11;
+ count=0;
+ }
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ 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;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ 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 += 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==2) {//横移動
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=3;
- mtc2=true;
- } else {
+ }
+ } else if(mode==11) {
+ 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;
+ if(g[2]==0) {
+ through=true;
+ }
+ if(through==true&&g[2]==255) {
+ through=false;
+ mode=12;
+ }
+ } else if(mode==12) {
+ motor[TIRE_FR].pwm = slow;
+ 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 = slow;
+ motor[TIRE_BL].dir = BACK;
+ if(g[2]==0) {
+ count=0;
+ cross=0;
+ mode=20;
+ }
+ } else if(mode==20) {
+ switch(g[2]) {
+ case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
@@ -457,1170 +702,1176 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- }
- } else if(mode==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;
- if(g[0]==0) {
- through=true;
- }
- if(through==true&&g[0]==255) {
- through=false;
- count=0;
- mode=4;
- }
- } else if(mode==4) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = slow;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
- count=0;
- cross=0;
- mode=10;
- }
- } else if(g[0]!=99&&mode==10) { //縦ライントレース
- switch(g[0]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==Twsh) {
- mode=11;
- count=0;
- }
- break;
+ if(count>70000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==3) {
+ mode=21;
+ count=0;
+ cross=0;
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 253:
+ 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;
+ mtc2=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 3:
+ 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;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[3]) {
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;
- mtc=true;
+ mtc2=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc=true;
+ mtc2=false;
break;
case 1:
- 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;
- mtc=true;
- break;
- case 3:
- 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;
- mtc=true;
- break;
- case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- 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 += 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==11) {
- 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;
- if(g[2]==0) {
- through=true;
- }
- if(through==true&&g[2]==255) {
- through=false;
- mode=12;
- }
- } else if(mode==12) {
- motor[TIRE_FR].pwm = slow;
- 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 = slow;
- motor[TIRE_BL].dir = BACK;
- if(g[2]==0) {
- count=0;
- cross=0;
- mode=20;
- }
- } else if(mode==20) {
- switch(g[2]) {
- case 98:
- 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(count>70000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==3) {
- mode=21;
- count=0;
- cross=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
- break;
- case 253:
- 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;
- mtc2=true;
- break;
- case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 3:
- 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;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 2:
- 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;
- mtc2=true;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc2=false;
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 += 20;
- 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 += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==21) {
+ 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;
+ if(g[0]==0) {
+ through=true;
+ }
+ if(through==true&&g[0]==255) {
+ through=false;
+ mode=22;
+ }
+ } else if(mode==22) {
+ motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].dir = FOR;
+ if(g[0]==0) {
+ solenoid.solenoid3 = SOLENOID_ON;
+ count=0;
+ cross=0;
+ mode=30;
+ }
+ } else if(mode==30) {
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==1) {
+ solenoid.solenoid3 = SOLENOID_OFF;
+ mode=31;
+ count=0;
}
- }
- } else if(mode==21) {
- 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;
- if(g[0]==0) {
- through=true;
- }
- if(through==true&&g[0]==255) {
- through=false;
- mode=22;
- }
- } else if(mode==22) {
- motor[TIRE_FR].pwm = slow;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
- solenoid.solenoid3 = SOLENOID_ON;
- count=0;
- cross=0;
- mode=30;
- }
- } else if(mode==30) {
- switch(g[0]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- solenoid.solenoid3 = SOLENOID_OFF;
- mode=31;
- count=0;
- }
- break;
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ 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;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ 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:
- 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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
break;
case 254:
- 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;
- mtc=true;
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
break;
case 3:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
break;
case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
break;
}
- if(g[0]!=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==31) {
+ motor[TIRE_FR].pwm = 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 = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ mode=32;
+ }
+ } else if(mode==32) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
+ mode=40;
+ }
+ } else if(mode==40) {
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ if(count>30000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==1) {
+ mode=41;
+ count=0;
+ cross=0;
}
- }
- } else if(mode==31) {
- motor[TIRE_FR].pwm = 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 = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
- count=0;
- mode=32;
- }
- } else if(mode==32) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[3]==0) {
- count=0;
- cross=0;
- mode=40;
- }
- } else if(mode==40) {
- switch(g[3]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- if(count>30000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==1) {
- mode=41;
- count=0;
- cross=0;
- }
- break;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[2]) {
case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ mtc2=false;
break;
case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ mtc2=false;
break;
case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 3:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc2=true;
+ mtc2=false;
break;
}
- if(g[2]!=98&&mtc2==true) {
- switch(g[2]) {
- case 0:
- mtc2=false;
- break;
- case 255:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==41) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=42;
+ }
+ } else if(mode==42) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=50;
+ }
+ } else if(mode==50) {
+ switch(g[1]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==1) {
+ mode=51;
+ count=0;
}
- }
- } else if(mode==41) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=42;
- }
- } else if(mode==42) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=50;
- }
- } else if(mode==50) {
- switch(g[1]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- mode=51;
- count=0;
- }
- break;
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ 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;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ 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:
- 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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc=false;
break;
case 253:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
break;
case 254:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
break;
case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
break;
case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
+ mtc=false;
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==51) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ mode=52;
+ }
+ } else if(mode==52) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
+ mode=60;
+ }
+ } else if(mode==60) {
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ if(count>50000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=61;
+ count=0;
+ cross=0;
}
- }
- } else if(mode==51) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[3]==0) {
- count=0;
- mode=52;
- }
- } else if(mode==52) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
- count=0;
- cross=0;
- mode=60;
- }
- } else if(mode==60) {
- switch(g[3]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- if(count>50000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==2) {
- mode=61;
- count=0;
- cross=0;
- }
- break;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[2]) {
case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ mtc2=false;
break;
case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ mtc2=false;
break;
case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 3:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc2=true;
+ mtc2=false;
break;
}
- if(g[2]!=98&&mtc2==true) {
- switch(g[2]) {
- case 0:
- mtc2=false;
- break;
- case 255:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==61) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=62;
+ }
+ } else if(mode==62) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=70;
+ }
+ } else if(mode==70) {
+ switch(g[1]) {
+ case 99:
+ if(count>=100000) {
+ mode=100;
}
- }
- } else if(mode==61) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=62;
- }
- } else if(mode==62) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=70;
- }
- } else if(mode==70) {
- switch(g[1]) {
- case 99:
- if(count>=100000) {
- mode=100;
- }
- break;
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- break;
+ break;
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ break;
+ case 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ 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;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ 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:
- 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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc=false;
break;
case 253:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
break;
case 254:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
break;
case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
break;
case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
+ mtc=false;
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==100) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 23;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = BACK;
+ }
+ } else if(mode==100) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 23;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BACK;
+ } else {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = BRAKE;
+ }
+}
+
+#endif
+
+#if USE_PROCESS_NUM>3
+static void Process3()
+{
+ for(int i=0; i<=3; i++) {
+ if(i==3) {
+ POTENTIOMETER::dio[3]=1;
} else {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = BRAKE;
+ POTENTIOMETER::dio[i]=0;
}
- } else {//////////////////////////////////////////////////これより別ゾーン用/////////////////////////////////////////////////////////////////////////////////
- if(mode==1) {//スタートゾーンから白線検知
+ }
+ if(mode==1) {//スタートゾーンから白線検知
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=2;
+ }
+ } else if(mode==2) {//横移動
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=3;
+ mtc2=true;
+ } else {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=2;
+ }
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ 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(count>100000&&g[0]==0) {
+ count=0;
+ mode=10;
+ }
+ } else if(mode==4) { //横ライントレースから縦ライントレースへ
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>30000&&g[0]==0) {
+ count=0;
+ cross=0;
+ mode=10;
+ }
+ } else if(g[0]!=99&&mode==10) { //縦ライントレース
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==Twsh) {
+ mode=11;
+ count=0;
+ }
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ 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;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ 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_BL].pwm += 5;
+ mtc=false;
+ break;
+ case 253:
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
+ break;
+ case 254:
+ motor[TIRE_BL].pwm += 20;
+ motor[TIRE_BR].pwm = 0;
+ mtc=false;
+ break;
+ case 1:
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
+ break;
+ case 3:
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
+ break;
+ case 2:
+ motor[TIRE_BR].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc=false;
+ break;
}
- } else if(mode==2) {//横移動
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=3;
- mtc2=true;
- } else {
+ }
+ } else if(mode==11) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[2]==0) {
+ count=0;
+ mode=12;
+ }
+ } else if(mode==12) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>10000&&g[2]==0) {
+ count=0;
+ cross=0;
+ mode=20;
+ }
+ } else if(mode==20) {
+ switch(g[2]) {
+ case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
@@ -1629,1149 +1880,942 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- }
- } else if(mode==3) { //横ライントレースから縦ライントレースへ
- 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(count>100000&&g[0]==0) {
- count=0;
- mode=10;
- }
- } else if(mode==4) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>30000&&g[0]==0) {
- count=0;
- cross=0;
- mode=10;
- }
- } else if(g[0]!=99&&mode==10) { //縦ライントレース
- switch(g[0]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==Twsh) {
- mode=11;
- count=0;
- }
- break;
+ if(count>70000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==3) {
+ mode=21;
+ count=0;
+ cross=0;
+ }
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 253:
+ 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;
+ mtc2=true;
+ break;
+ case 254:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = FOR;
+ 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 = BACK;
+ mtc2=true;
+ break;
+ case 3:
+ 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;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[3]) {
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;
- mtc=true;
+ mtc2=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 253:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc=true;
+ mtc2=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- 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 = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- 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_BL].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_BL].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_BR].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_BR].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_BR].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc=false;
- break;
- }
- }
- } else if(mode==11) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[2]==0) {
- count=0;
- mode=12;
- }
- } else if(mode==12) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>10000&&g[2]==0) {
- count=0;
- cross=0;
- mode=20;
- }
- } else if(mode==20) {
- switch(g[2]) {
- case 98:
- 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(count>70000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==3) {
- mode=21;
- count=0;
- cross=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
- break;
- case 253:
- 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;
- mtc2=true;
- break;
- case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- mtc2=true;
- break;
- case 3:
- 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;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 2:
- 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;
- mtc2=true;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_BL].pwm = 0;
+ mtc2=false;
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 += 20;
- 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 += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==21) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>2000&&g[0]==0) {
+ count=0;
+ mode=22;
+ }
+ } else if(mode==22) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[0]==0) {
+ count=0;
+ cross=0;
+ mode=30;
+ }
+ } else if(mode==30) {
+ switch(g[0]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==1) {
+ mode=31;
+ count=0;
}
- }
- } else if(mode==21) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>2000&&g[0]==0) {
- count=0;
- mode=22;
- }
- } else if(mode==22) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[0]==0) {
- count=0;
- cross=0;
- mode=30;
- }
- } else if(mode==30) {
- switch(g[0]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- mode=31;
- count=0;
- }
- break;
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BACK;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc=true;
+ break;
+ }
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_BR].pwm += 5;
+ mtc=false;
break;
case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ motor[TIRE_BR].pwm += 10;
+ mtc=false;
break;
case 2:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ mtc=false;
break;
}
- if(g[0]!=98&&mtc==true) {
- switch(g[1]) {
- case 0:
- mtc=false;
- break;
- case 255:
- motor[TIRE_BL].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_BL].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_BR].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_BR].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_BR].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc=false;
- break;
+ }
+ } else if(mode==31) {
+ motor[TIRE_FR].pwm = 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 = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ mode=32;
+ }
+ } else if(mode==32) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
+ mode=40;
+ }
+ } else if(mode==40) {
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ if(count>30000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==1) {
+ mode=41;
+ count=0;
+ cross=0;
}
- }
- } else if(mode==31) {
- motor[TIRE_FR].pwm = 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 = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
- count=0;
- mode=32;
- }
- } else if(mode==32) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[3]==0) {
- count=0;
- cross=0;
- mode=40;
- }
- } else if(mode==40) {
- switch(g[3]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- if(count>30000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==1) {
- mode=41;
- count=0;
- cross=0;
- }
- break;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[2]) {
case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ mtc2=false;
break;
case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ mtc2=false;
break;
case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 3:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc2=true;
+ mtc2=false;
break;
}
- if(g[2]!=98&&mtc2==true) {
- switch(g[2]) {
- case 0:
- mtc2=false;
- break;
- case 255:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==41) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=42;
+ }
+ } else if(mode==42) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=50;
+ }
+ } else if(mode==50) {
+ switch(g[1]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ if(count>100000) {
+ cross++;
+ count=0;
+ }
+ mtc=true;
+ if(cross==1) {
+ mode=51;
+ count=0;
}
- }
- } else if(mode==41) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=42;
- }
- } else if(mode==42) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=50;
- }
- } else if(mode==50) {
- switch(g[1]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- mode=51;
- count=0;
- }
- break;
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ 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;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ 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[0]) {
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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm += 20;
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;
- mtc=true;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc=false;
break;
case 3:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
break;
case 2:
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ mtc=false;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[0]) {
- case 0:
- mtc=false;
- break;
- case 255:
- motor[TIRE_FR].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_FR].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_FL].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_FL].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_FR].pwm = 0;
- mtc=false;
- break;
+ }
+ } else if(mode==51) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ mode=52;
+ }
+ } else if(mode==52) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
+ mode=60;
+ }
+ } else if(mode==60) {
+ switch(g[3]) {
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = FOR;
+ if(count>50000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=61;
+ count=0;
+ cross=0;
}
- }
- } else if(mode==51) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[3]==0) {
- count=0;
- mode=52;
- }
- } else if(mode==52) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
- count=0;
- cross=0;
- mode=60;
- }
- } else if(mode==60) {
- switch(g[3]) {
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- if(count>50000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==2) {
- mode=61;
- count=0;
- cross=0;
- }
- break;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 255:
+ motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ 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 = 30;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ mtc2=true;
+ break;
+ case 1:
+ motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ mtc2=true;
+ break;
+ case 3:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc2=true;
+ break;
+ case 2:
+ 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;
+ mtc2=true;
+ break;
+ }
+ if(g[2]!=98&&mtc2==true) {
+ switch(g[2]) {
case 0:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ mtc2=false;
break;
case 255:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc2=false;
break;
case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc2=false;
break;
case 254:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
+ mtc2=false;
break;
case 1:
- motor[TIRE_FR].pwm = 60;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 5;
+ mtc2=false;
break;
case 3:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
+ motor[TIRE_BL].pwm += 10;
+ mtc2=false;
break;
case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_BL].pwm += 20;
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;
- mtc2=true;
+ mtc2=false;
break;
}
- if(g[2]!=98&&mtc2==true) {
- switch(g[2]) {
- case 0:
- mtc2=false;
- break;
- case 255:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
+ }
+ } else if(mode==61) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=62;
+ }
+ } else if(mode==62) {
+ motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = BACK;
+ if(count>5000&&g[1]==0) {
+ count=0;
+ mode=70;
+ }
+ } else if(mode==70) {
+ switch(g[1]) {
+ case 99:
+ if(count>=100000) {
+ mode=100;
}
- }
- } else if(mode==61) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=62;
- }
- } else if(mode==62) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=70;
- }
- } else if(mode==70) {
- switch(g[1]) {
- case 99:
- if(count>=100000) {
- mode=100;
- }
- break;
- case 98:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- break;
+ break;
+ case 98:
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ 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 = FOR;
+ 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;
+ mtc=true;
+ break;
+ case 255:
+ 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;
+ mtc=true;
+ break;
+ case 253:
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = FOR;
+ mtc=true;
+ break;
+ case 254:
+ 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;
+ mtc=true;
+ break;
+ case 1:
+ 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;
+ mtc=true;
+ break;
+ case 3:
+ 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;
+ mtc=true;
+ break;
+ case 2:
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ 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[0]) {
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;
- mtc=true;
+ mtc=false;
break;
case 255:
- 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;
- mtc=true;
+ motor[TIRE_FR].pwm += 5;
+ mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm = 20;
- 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 = 20;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
+ motor[TIRE_FR].pwm += 10;
+ mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm += 20;
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;
- mtc=true;
+ mtc=false;
break;
case 1:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 5;
+ mtc=false;
break;
case 3:
- 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;
- mtc=true;
+ motor[TIRE_FL].pwm += 10;
+ mtc=false;
break;
case 2:
+ motor[TIRE_FL].pwm += 20;
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
+ mtc=false;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[0]) {
- case 0:
- mtc=false;
- break;
- case 255:
- motor[TIRE_FR].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_FR].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_FL].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_FL].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_FR].pwm = 0;
- mtc=false;
- break;
- }
- }
- } else if(mode==100) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 23;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = BACK;
- } else {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = BRAKE;
}
+ } else if(mode==100) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 23;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BACK;
+ } else {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = BRAKE;
}
-
}
#endif
-#if USE_PROCESS_NUM>3
-static void Process3()
-{
-
-}
-#endif
-
#if USE_PROCESS_NUM>4
static void Process4()
{
--- a/System/Process/Process.h Fri Sep 20 02:47:02 2019 +0000
+++ b/System/Process/Process.h Sat Sep 21 04:28:31 2019 +0000
@@ -17,7 +17,9 @@
#define StertSW LimitSw::IsPressed(9) //スタートスイッチ
#define ZoneSW LimitSw::IsPressed(10) //赤ゾーン用スイッチ
-#define ThSW LimitSw::IsPressed(4) //シーツタオルスイッチ
+#define ThSW LimitSw::IsPressed(4) //シーツタオルスイッチ
+#define UPSW LimitSw::IsPressed(2) //上下機構トリガー
+#define UPLS LimitSw::IsPressed(5) //上下機構リミット
typedef union
{