大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 32:90f018b10ad6
- Parent:
- 31:45f97698857b
- Child:
- 33:656aa14904a5
--- a/System/Process/Process.cpp Sat Sep 21 04:28:31 2019 +0000
+++ b/System/Process/Process.cpp Sat Sep 21 11:28:06 2019 +0000
@@ -242,7 +242,9 @@
for(int i = 0; i < 8; i++) {
g[i] = LineHub::GetPara(i);
}
-
+
+ //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]);
+
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
@@ -1685,13 +1687,13 @@
}
} else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
+ motor[TIRE_BL].dir = BACK;
if(count>100000&&g[0]==0) {
count=0;
mode=10;
@@ -1710,8 +1712,8 @@
cross=0;
mode=10;
}
- } else if(g[0]!=99&&mode==10) { //縦ライントレース
- switch(g[0]) {
+ } else if(g[1]!=99&&mode==10) { //縦ライントレース
+ switch(g[1]) {
case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
@@ -1809,61 +1811,61 @@
mtc=true;
break;
}
- if(g[0]!=98&&mtc==true) {
- switch(g[1]) {
+ if(g[1]!=98&&mtc==true) {
+ switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
- motor[TIRE_BL].pwm += 5;
+ motor[TIRE_FL].pwm += 5;
mtc=false;
break;
case 253:
- motor[TIRE_BL].pwm += 10;
+ motor[TIRE_FL].pwm += 10;
mtc=false;
break;
case 254:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_BR].pwm = 0;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
- motor[TIRE_BR].pwm += 5;
+ motor[TIRE_FR].pwm += 5;
mtc=false;
break;
case 3:
- motor[TIRE_BR].pwm += 10;
+ motor[TIRE_FR].pwm += 10;
mtc=false;
break;
case 2:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_BL].pwm = 0;
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
}
} else if(mode==11) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
+ motor[TIRE_BL].dir = BACK;
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_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
if(count>10000&&g[2]==0) {
count=0;
cross=0;
@@ -2004,14 +2006,14 @@
}
} else if(mode==21) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>2000&&g[0]==0) {
+ motor[TIRE_BL].dir = BACK;
+ if(count>2000&&g[1]==0) {
count=0;
mode=22;
}
@@ -2024,13 +2026,13 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[0]==0) {
+ if(count>5000&&g[1]==0) {
count=0;
cross=0;
mode=30;
}
} else if(mode==30) {
- switch(g[0]) {
+ switch(g[1]) {
case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
@@ -2128,8 +2130,8 @@
mtc=true;
break;
}
- if(g[0]!=98&&mtc==true) {
- switch(g[1]) {
+ if(g[1]!=98&&mtc==true) {
+ switch(g[0]) {
case 0:
mtc=false;
break;
@@ -2163,26 +2165,26 @@
}
} else if(mode==31) {
motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = FREE;
+ motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].dir = FREE;
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_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
if(count>5000&&g[3]==0) {
count=0;
cross=0;
@@ -2323,14 +2325,14 @@
}
} else if(mode==41) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[0]==0) {
count=0;
mode=42;
}
@@ -2343,12 +2345,12 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
+ if(count>5000&&g[0]==0) {
count=0;
mode=50;
}
} else if(mode==50) {
- switch(g[1]) {
+ switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
@@ -2446,8 +2448,8 @@
mtc=true;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[0]) {
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
case 0:
mtc=false;
break;
@@ -2481,26 +2483,26 @@
}
} else if(mode==51) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
+ motor[TIRE_BL].dir = FOR;
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_FR].dir = FOR;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
if(count>5000&&g[3]==0) {
count=0;
cross=0;
@@ -2641,14 +2643,14 @@
}
} else if(mode==61) {
motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
+ motor[TIRE_BL].dir = FOR;
+ if(count>5000&&g[0]==0) {
count=0;
mode=62;
}
@@ -2661,12 +2663,12 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[1]==0) {
+ if(count>5000&&g[0]==0) {
count=0;
mode=70;
}
} else if(mode==70) {
- switch(g[1]) {
+ switch(g[0]) {
case 99:
if(count>=100000) {
mode=100;
@@ -2760,8 +2762,8 @@
mtc=true;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[0]) {
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
case 0:
mtc=false;
break;
@@ -2795,13 +2797,13 @@
}
} else if(mode==100) {
motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 23;
- motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
} else {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;