大季 矢花
daad
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 24:41c23c9a5058
- Parent:
- 23:694ecfa97839
--- a/System/Process/Process.cpp Sun Sep 15 11:53:56 2019 +0000
+++ b/System/Process/Process.cpp Mon Sep 16 00:03:18 2019 +0000
@@ -1048,140 +1048,489 @@
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;
+ }
+ } 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:
- 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_FR].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_FR].pwm += 10;
+ mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
+ 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 = 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_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 = 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_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 = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- 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 = 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;
+ 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;
+ mode1g0++;
+ if(mode1g0>5000&&g[3]==0) {
+ mode1g0=0;
+ mode1=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(mode1g0>100000) {
+ mode1++;
+ mode1g0=0;
+ }
+ mtc2=true;
+ if(mode1==1) {
+ mode=61;
+ 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==61) {
+ 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=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(mode1g0>5000&&g[1]==0) {
+ mode1g0=0;
+ mode=70;
+ }
+ } else if(mode==70) {
+ 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=71;
+ 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==71) {
+ 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;
+ 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;
@@ -1190,7 +1539,6 @@
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;