大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 27:545148741493
- Parent:
- 26:be5121da0f77
- Child:
- 28:9cd36721cf83
--- a/System/Process/Process.cpp Tue Sep 17 12:02:53 2019 +0000
+++ b/System/Process/Process.cpp Wed Sep 18 03:07:18 2019 +0000
@@ -270,13 +270,14 @@
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
+
static void Process0()
{
if(StertSW) {
if(ZoneSW) {
current = 1;
Twsh=2;
- }else{
+ } else {
current = 1;
Twsh=3;
}
@@ -286,7 +287,6 @@
#if USE_PROCESS_NUM>1
-
bool dz1=true;
bool dz1i=false;
bool dz2=true;
@@ -296,17 +296,16 @@
bool dz4=true;
bool dz4i=false;
-//int mode=10;
-int mode =10;
-int mode1=0;
-int mode1g0=100000;
+int mode =1;
+int cross=0;//十字数える用
+int count=100000;//wait代替え
static void Process1()
{
- mode1g0=100000;
+ count=100000;
mode=1;
- mode1=0;
+ 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]);
motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
@@ -416,12 +415,9 @@
static void Process2()
{
-//追加
-//Twsh = 2;
// 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(mode==1) {
+ count++;
+ if(mode==1) {//スタートゾーンから白線検知
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
@@ -430,157 +426,41 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(mode1g0>100000&&g[2]==0) {//変更5000→100000
- mode1g0=0;
+ if(g[2]==0) {
+ count=100000;
+ cross=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;
+ } 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 = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
}
- 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) {
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 20;
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;
+ if(count>100000&&g[0]==0) {
+ count=0;
+ mode=10;
}
- } else if(mode==4) {
+ } else if(mode==4) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
@@ -589,12 +469,12 @@
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;
+ if(count>30000&&g[0]==0) {
+ count=0;
+ cross=0;
mode=10;
}
- } else if(g[0]!=99&&mode==10) {
+ } else if(g[0]!=99&&mode==10) { //縦ライントレース
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = 30;
@@ -605,14 +485,14 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>100000) {
+ cross++;
+ count=0;
}
mtc=true;
- if(mode1==Twsh) {
+ if(cross==1) {
mode=11;
- mode1g0=0;
+ count=0;
}
break;
case 0:
@@ -735,8 +615,8 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(mode1g0>5000&&g[2]==0) {
- mode1g0=0;
+ if(count>5000&&g[2]==0) {
+ count=0;
mode=12;
}
} else if(mode==12) {
@@ -748,10 +628,9 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- mode1g0++;
- if(mode1g0>10000&&g[2]==0) {
- mode1g0=0;
- mode1=0;
+ if(count>10000&&g[2]==0) {
+ count=0;
+ cross=0;
mode=20;
}
} else if(mode==20) {
@@ -765,15 +644,15 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>50000) {
+ cross++;
+ count=0;
}
mtc2=true;
- if(mode1==3) {
+ if(cross==3) {
mode=21;
- mode1g0=0;
- mode1=0;
+ count=0;
+ cross=0;
}
break;
case 0:
@@ -896,8 +775,8 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(mode1g0>5000&&g[0]==0) {
- mode1g0=0;
+ if(count>5000&&g[0]==0) {
+ count=0;
mode=22;
}
} else if(mode==22) {
@@ -909,9 +788,9 @@
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;
+ if(count>10000&&g[0]==0) {
+ count=0;
+ cross=0;
mode=30;
}
} else if(mode==30) {
@@ -925,14 +804,14 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>100000) {
+ cross++;
+ count=0;
}
mtc=true;
- if(mode1==1) {
+ if(cross==1) {
mode=31;
- mode1g0=0;
+ count=0;
}
break;
case 0:
@@ -1055,9 +934,8 @@
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = FOR;
- mode1g0++;
- if(mode1g0>5000&&g[3]==0) {
- mode1g0=0;
+ if(count>5000&&g[3]==0) {
+ count=0;
mode=32;
}
} else if(mode==32) {
@@ -1069,10 +947,9 @@
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;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
mode=40;
}
} else if(mode==40) {
@@ -1086,15 +963,15 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>30000) {
+ cross++;
+ count=0;
}
mtc2=true;
- if(mode1==1) {
+ if(cross==1) {
mode=41;
- mode1g0=0;
- mode1=0;
+ count=0;
+ cross=0;
}
break;
case 0:
@@ -1217,8 +1094,8 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(mode1g0>5000&&g[1]==0) {
- mode1g0=0;
+ if(count>5000&&g[1]==0) {
+ count=0;
mode=42;
}
} else if(mode==42) {
@@ -1230,8 +1107,8 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- if(mode1g0>5000&&g[1]==0) {
- mode1g0=0;
+ if(count>5000&&g[1]==0) {
+ count=0;
mode=50;
}
} else if(mode==50) {
@@ -1245,14 +1122,14 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>100000) {
+ cross++;
+ count=0;
}
mtc=true;
- if(mode1==1) {
+ if(cross==1) {
mode=51;
- mode1g0=0;
+ count=0;
}
break;
case 0:
@@ -1339,29 +1216,29 @@
mtc=false;
break;
case 255:
- motor[TIRE_FR].pwm += 5;
+ motor[TIRE_FL].pwm += 5;
mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm += 10;
+ motor[TIRE_FL].pwm += 10;
mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
- motor[TIRE_FL].pwm += 5;
+ motor[TIRE_FR].pwm += 5;
mtc=false;
break;
case 3:
- motor[TIRE_FL].pwm += 10;
+ motor[TIRE_FR].pwm += 10;
mtc=false;
break;
case 2:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
@@ -1375,9 +1252,8 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- mode1g0++;
- if(mode1g0>5000&&g[3]==0) {
- mode1g0=0;
+ if(count>5000&&g[3]==0) {
+ count=0;
mode=52;
}
} else if(mode==52) {
@@ -1389,10 +1265,9 @@
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;
+ if(count>5000&&g[3]==0) {
+ count=0;
+ cross=0;
mode=60;
}
} else if(mode==60) {
@@ -1406,15 +1281,15 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>50000) {
+ cross++;
+ count=0;
}
mtc2=true;
- if(mode1==1) {
+ if(cross==2) {
mode=61;
- mode1g0=0;
- mode1=0;
+ count=0;
+ cross=0;
}
break;
case 0:
@@ -1537,8 +1412,8 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(mode1g0>5000&&g[1]==0) {
- mode1g0=0;
+ if(count>5000&&g[1]==0) {
+ count=0;
mode=62;
}
} else if(mode==62) {
@@ -1550,8 +1425,8 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- if(mode1g0>5000&&g[1]==0) {
- mode1g0=0;
+ if(count>5000&&g[1]==0) {
+ count=0;
mode=70;
}
} else if(mode==70) {
@@ -1565,14 +1440,14 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- if(mode1g0>100000) {
- mode1++;
- mode1g0=0;
+ if(count>100000) {
+ cross++;
+ count=0;
}
mtc=true;
- if(mode1==1) {
+ if(cross==1) {
mode=71;
- mode1g0=0;
+ count=0;
}
break;
case 0:
@@ -1659,29 +1534,29 @@
mtc=false;
break;
case 255:
- motor[TIRE_FR].pwm += 5;
+ motor[TIRE_FL].pwm += 5;
mtc=false;
break;
case 253:
- motor[TIRE_FR].pwm += 10;
+ motor[TIRE_FL].pwm += 10;
mtc=false;
break;
case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].pwm += 20;
+ motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
- motor[TIRE_FL].pwm += 5;
+ motor[TIRE_FR].pwm += 5;
mtc=false;
break;
case 3:
- motor[TIRE_FL].pwm += 10;
+ motor[TIRE_FR].pwm += 10;
mtc=false;
break;
case 2:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].pwm += 20;
+ motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
@@ -1695,12 +1570,11 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = BACK;
- mode1g0++;
- if(mode1g0>5000&&g[3]==0) {
- mode1g0=0;
+ if(count>5000&&g[3]==0) {
+ count=0;
mode=72;
}
- }else if(mode==72) {
+ } else if(mode==72) {
motor[TIRE_FR].pwm = 15;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 15;
@@ -1709,13 +1583,25 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 15;
motor[TIRE_BL].dir = FOR;
- mode1g0++;
- if(mode1g0>100000&&g[3]==0) {
- mode1g0=0;
- mode1=0;
+ count++;
+ if(count>50000&&g[3]==0) {
+ count=0;
+ cross=0;
mode=80;
}
- }else if(mode==80) {
+ } else if(mode==80) {
+ if(count>=400000) {
+ mode=100;
+ }
+ motor[TIRE_FR].pwm = 40;
+ 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 = 40;
+ motor[TIRE_BL].dir = BACK;
+ } else if(mode==100) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 0;
@@ -1724,7 +1610,7 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = BRAKE;
- }else {
+ } else {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 0;