大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 23:694ecfa97839
- Parent:
- 22:c47f29caded6
--- a/System/Process/Process.cpp Sat Sep 14 00:10:57 2019 +0000
+++ b/System/Process/Process.cpp Sun Sep 15 11:53:56 2019 +0000
@@ -288,7 +288,7 @@
bool dz4=true;
bool dz4i=false;
-int mode=0;
+int mode=10;
int mode1=0;
int mode1g0=100000;
@@ -296,7 +296,7 @@
static void Process1()
{
mode1g0=100000;
- mode=0;
+ mode=10;
mode1=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]);
@@ -409,7 +409,7 @@
{
// 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]);
mode1g0++;
- if(g[0]!=99&&mode==0) {
+ if(g[0]!=99&&mode==10) {
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = 30;
@@ -426,7 +426,8 @@
}
mtc=true;
if(mode1==2) {
- mode=1;
+ mode=11;
+ mode1g0=0;
}
break;
case 0:
@@ -540,19 +541,20 @@
break;
}
}
- } else if(mode==1) {
- motor[TIRE_FR].pwm = 5;
+ } else if(mode==11) {
+ motor[TIRE_FR].pwm = 15;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 5;
+ motor[TIRE_FL].pwm = 15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 5;
+ motor[TIRE_BR].pwm = 15;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 5;
+ motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = FOR;
- if(g[2]==0) {
- mode=2;
+ if(mode1g0>5000&&g[2]==0) {
+ mode1g0=0;
+ mode=12;
}
- } else if(mode==2) {
+ } else if(mode==12) {
motor[TIRE_FR].pwm = 15;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 15;
@@ -563,10 +565,12 @@
motor[TIRE_BL].dir = BACK;
mode1g0++;
if(mode1g0>10000&&g[2]==0) {
- mode=3;
+ mode1g0=0;
+ mode1=0;
+ mode=20;
}
- } else if(mode==3) {
- switch(g[2]) {
+ } else if(mode==20) {
+ switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
@@ -576,18 +580,15 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
-
- mtc2=true;
-
if(mode1g0>100000) {
mode1++;
mode1g0=0;
}
-
mtc2=true;
-
- if(mode1==2) {
- mode=3;
+ if(mode1==3) {
+ mode=21;
+ mode1g0=0;
+ mode1=0;
}
break;
case 0:
@@ -701,9 +702,496 @@
break;
}
}
- }
-
- else {
+ } else if(mode==21) {
+ 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(mode1g0>5000&&g[0]==0) {
+ mode1g0=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(mode1g0>10000&&g[0]==0) {
+ mode1g0=0;
+ mode1=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(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc=true;
+ if(mode1==1) {
+ mode=31;
+ mode1g0=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==31) {
+ 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;
+ mode1g0++;
+ if(mode1g0>5000&&g[3]==0) {
+ mode1g0=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;
+ mode1g0++;
+ if(mode1g0>5000&&g[3]==0) {
+ mode1g0=0;
+ mode1=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(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc2=true;
+ if(mode1==1) {
+ mode=41;
+ mode1g0=0;
+ mode1=0;
+ }
+ 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:
+ 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 = 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(mode1g0>5000&&g[1]==0) {
+ mode1g0=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(mode1g0>5000&&g[1]==0) {
+ mode1g0=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(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc=true;
+ if(mode1==1) {
+ mode=51;
+ mode1g0=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[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 = 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==52) {
+ } else {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 0;
@@ -715,11 +1203,13 @@
}
}
+
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
+
}
#endif