大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 25:38d4964e8aad
- Parent:
- 24:41c23c9a5058
- Child:
- 26:be5121da0f77
diff -r 41c23c9a5058 -r 38d4964e8aad System/Process/Process.cpp
--- a/System/Process/Process.cpp Mon Sep 16 00:03:18 2019 +0000
+++ b/System/Process/Process.cpp Tue Sep 17 03:46:34 2019 +0000
@@ -265,14 +265,22 @@
}
-
+int Twsh;
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
-
+ if(StertSW) {
+ if(ZoneSW) {
+ current = 1;
+ Twsh=2;
+ } else {
+ current = 1;
+ Twsh=3;
+ }
+ }
}
#endif
@@ -296,7 +304,7 @@
static void Process1()
{
mode1g0=100000;
- mode=10;
+ mode=1;
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 +417,180 @@
{
// 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==10) {
+ if(mode==1) {
+ 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(mode1g0>5000&&g[2]==0) {
+ mode1g0=0;
+ mode=2;
+ }
+ } else if(mode==2) {
+ 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(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc2=true;
+ if(mode1==1) {
+ mode=3;
+ mode1g0=0;
+ mode1=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:
+ 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==3) {
+ 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(mode1g0>5000&&g[0]==0) {
+ mode1g0=0;
+ mode=4;
+ }
+ } 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(mode1g0>10000&&g[0]==0) {
+ mode1g0=0;
+ mode1=0;
+ mode=10;
+ }
+ } else if(g[0]!=99&&mode==10) {
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = 30;
@@ -425,7 +606,7 @@
mode1g0=0;
}
mtc=true;
- if(mode1==2) {
+ if(mode1==Twsh) {
mode=11;
mode1g0=0;
}
@@ -542,14 +723,14 @@
}
}
} else if(mode==11) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
+ 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 = 15;
- motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
if(mode1g0>5000&&g[2]==0) {
mode1g0=0;
mode=12;
@@ -703,14 +884,14 @@
}
}
} else if(mode==21) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
+ 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 = 15;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
if(mode1g0>5000&&g[0]==0) {
mode1g0=0;
mode=22;
@@ -864,10 +1045,10 @@
} 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_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;
mode1g0++;
@@ -1024,14 +1205,14 @@
}
}
} else if(mode==41) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
+ 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 = 15;
- motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
if(mode1g0>5000&&g[1]==0) {
mode1g0=0;
mode=42;
@@ -1182,14 +1363,14 @@
}
}
} else if(mode==51) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
+ 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 = 15;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
mode1g0++;
if(mode1g0>5000&&g[3]==0) {
mode1g0=0;
@@ -1344,14 +1525,14 @@
}
}
} else if(mode==61) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
+ 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 = 15;
- motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
if(mode1g0>5000&&g[1]==0) {
mode1g0=0;
mode=62;
@@ -1510,35 +1691,6 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- mode1g0++;
- if(mode1g0>5000&&g[3]==0) {
- mode1g0=0;
- mode=72;
- }
- } else if(mode==72) {
- 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;
- mode1=0;
- mode=80;
- }
- } else if(mode==80) {
- 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 {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;