大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 42:4f9689c0a349
- Parent:
- 41:e1dd6d97f17d
- Child:
- 43:677f7446612e
diff -r e1dd6d97f17d -r 4f9689c0a349 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sun Sep 29 07:28:05 2019 +0000
+++ b/System/Process/Process.cpp Mon Sep 30 11:33:22 2019 +0000
@@ -104,63 +104,33 @@
else return abs(pwmVal);
}
-void LedMode(int led)
+int selectnum[16][4]= {
+//1,2,4,8
+ {0,0,0,0},
+ {1,0,0,0},
+ {0,1,0,0},
+ {1,1,0,0},
+ {0,0,1,0},
+ {1,0,1,0},
+ {0,1,1,0},
+ {1,1,1,0},
+ {0,0,0,1},
+ {1,0,0,1},
+ {0,1,0,1},
+ {1,1,0,1},
+ {0,0,1,1},
+ {1,0,1,1},
+ {0,1,1,1},
+ {1,1,1,1}
+};
+
+void LedMode(int num)
{
- /*
- switch(led) {
- case 1:
- POTENTIOMETER::dio[0]= 1;
- POTENTIOMETER::dio[1]= 0;
- POTENTIOMETER::dio[2]= 0;
- POTENTIOMETER::dio[3]= 0;
- break;
- case 2:
- POTENTIOMETER::dio[0]= 0;
- POTENTIOMETER::dio[1]= 1;
- POTENTIOMETER::dio[2]= 0;
- POTENTIOMETER::dio[3]= 0;
- break;
- case 3:
- POTENTIOMETER::dio[0]= 1;
- POTENTIOMETER::dio[1]= 1;
- POTENTIOMETER::dio[2]= 0;
- POTENTIOMETER::dio[3]= 0;
- break;
- case 4:
- POTENTIOMETER::dio[0]= 0;
- POTENTIOMETER::dio[1]= 0;
- POTENTIOMETER::dio[2]= 1;
- POTENTIOMETER::dio[3]= 0;
- break;
- }
- */
- if(led/8>=1) {
- POTENTIOMETER::dio[3]= 1;
- led=led%8;
- } else {
- POTENTIOMETER::dio[3]= 0;
- }
+ POTENTIOMETER::dio[0]= selectnum[num][0];
+ POTENTIOMETER::dio[1]= selectnum[num][1];;
+ POTENTIOMETER::dio[2]= selectnum[num][2];;
+ POTENTIOMETER::dio[3]= selectnum[num][3];;
- if(led/4>=1) {
- POTENTIOMETER::dio[2]= 1;
- led=led%4;
- } else {
- POTENTIOMETER::dio[2]= 0;
- }
-
- if(led/2>=1) {
- POTENTIOMETER::dio[1]= 1;
- led=led%2;
- } else {
- POTENTIOMETER::dio[1]= 0;
- }
-
- if(led/1>=1) {
- POTENTIOMETER::dio[0]= 1;
- led=0;
- } else {
- POTENTIOMETER::dio[0]= 0;
- }
}
int Twsh;
bool UP_flag = false;
@@ -184,7 +154,7 @@
bool start_flag = true;
-int mode =1;
+int mode =0;
int cross=0;//十字数える用
int cros=0;
int count=100000;//wait代替え
@@ -310,6 +280,13 @@
SystemProcessInitialize();
while(1) {
+ if(mode==0) {
+ if(ZoneSW) {
+ LedMode(1);
+ } else {
+ LedMode(2);
+ }
+ }
for(int i = 0; i < 8; i++) {
g[i] = LineHub::GetPara(i);
}
@@ -318,18 +295,18 @@
} else {
Twsh=2;
}
+
if(StertSW && start_flag) {
- start_flag = false;
- lock = false;
- LedMode(1);
- if(ZoneSW) {
- mode=1;
- current = 2;
- } else {
- mode=1;
- current = 5;
- }
+ start_flag = false;
+ lock = false;
+ if(ZoneSW) {
+ mode=1;
+ current = 2;
+ } else {
+ mode=1;
+ current = 5;
}
+ }
if(DWSW||DOSW) {
if(DWSW) {
zyouge=true;
@@ -343,7 +320,7 @@
motor[MOTOR_0].dir = BACK;
}
}
- } else if(zyouge==false){
+ } else if(zyouge==false) {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
}
@@ -380,7 +357,13 @@
}
}
} else {
- SW_flag=false;
+ if( SW_flag==true) {
+ count++;
+ if(count>=5000) {
+ SW_flag=false;
+ count=0;
+ }
+ }
}
//printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
@@ -430,7 +413,7 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
- ///*
+ /*
mode=1;
LedMode(1);
if(ThSW) {
@@ -501,7 +484,7 @@
zyougedo=false;
}
}
- //*/
+ */
}
#endif
@@ -509,11 +492,10 @@
#if USE_PROCESS_NUM>1
static void Process1()
{
+ LedMode(6);
start_flag = true;
-
mtc=false;
mtc2 = false;
- LedMode(2);
mode=1;
cross=0;
Twsh=1;
@@ -612,10 +594,13 @@
#if USE_PROCESS_NUM>2
static void Process2()
{
+// 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]);
LedMode(3);
-// 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]);
count++;
if(mode==1) {//スタートゾーンから白線検知
+ fast=40;
+ normal=30;
+ slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
@@ -632,9 +617,9 @@
} else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].pwm = 25;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = 25;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -833,12 +818,12 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- if(count>80000) {
+ if(count>50000) {
cross++;
count=0;
}
mtc2=true;
- if(cross==2) {
+ if(cross==1) {
mode=21;
count=0;
cross=0;
@@ -956,6 +941,44 @@
}
}
} else if(mode==21) {
+ motor[TIRE_FR].pwm = 26;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 26;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BACK;
+ if(g[2]==98&&count>=10000) {
+ mode=22;
+ count=0;
+ }
+ } else if(mode==22) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ if(g[2]!=98) {
+ count=0;
+ mode=23;
+ }
+ } else if(mode==23) {
+ motor[TIRE_FR].pwm = 10;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 10;
+ motor[TIRE_BL].dir = BRAKE;
+ if(count<=100000) {
+ mode=30;
+ }
+ } else if(mode==24) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
@@ -967,205 +990,55 @@
if(g[0]==0) {
mode=30;
cros=0;
- zyouge=true;
}
} else if(mode==30) {
- motor[TIRE_FR].pwm = 62;
+ motor[TIRE_FR].pwm = 63;
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 = 62;
+ motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
if(g[0]==98) {
mode=31;
- }
- if(g[3]==0) {
- mode=32;
- count=0;
- cros=0;
- }//ゆっくりモードに入れなかった時の保険
- /*
- if(g[0]==98) {
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_OFF;
- }
- mode=31;
count=0;
}
- 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(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_OFF;
- }
- mode=31;
- count=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = 80;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 80;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 80;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 80;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = 80;
- 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 = 80;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = 80;
- 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 = 40;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 254:
- motor[TIRE_FR].pwm = 50;
- 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 = 50;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 80;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 80;
- 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 = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 40;
- 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 = 50;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 50;
- 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 = 10;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 10;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- if(g[3]==0) {
- count=0;
- mode=40;
- }
- */
-
} 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;
- if(g[3]==0) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>=5000) {
count=0;
mode=32;
}
} else if(mode==32) {
+ motor[TIRE_FR].pwm = 10;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 10;
+ motor[TIRE_BL].dir = BRAKE;
+ if(count>=40000) {
+ mode=33;
+ count=0;
+ }
+ } else if(mode==33) {
motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
if(g[3]==0) {
count=0;
if(Twsh==2) {
@@ -1206,7 +1079,7 @@
count=0;
}
mtc2=true;
- if(cross==2) {
+ if(cross==1) {
mode=41;
count=0;
cross=0;
@@ -1324,6 +1197,9 @@
}
}
} else if(mode==41) {
+ if(LIF||LIB) {
+ mode=42;
+ }
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 30;
@@ -2413,7 +2289,7 @@
#if USE_PROCESS_NUM>5
static void Process5()
{
- LedMode(5);
+ LedMode(4);
count++;
if(mode==1) {//スタートゾーンから白線検知
fast=50;
@@ -2763,6 +2639,7 @@
break;
}
}
+
} else if(mode==21) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BRAKE;
@@ -2775,19 +2652,40 @@
if(count<=100000) {
mode=22;
}
- } else if(mode==22) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FOR;
- if(g[2]!=98) {
- mode=30;
+ }
+ if(YELLOW) {
+ if(mode==22) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ if(g[2]!=98) {
+ mode=30;
+ }
}
- } else if(mode==30) {
+
+ } else {
+ if(mode==22) {
+ motor[TIRE_FR].pwm = normal;
+ 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 = normal;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
+ count=0;
+ cross=0;
+ mode=30;
+ }
+ }
+ }
+ if(mode==30) {
motor[TIRE_FR].pwm = 64;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
@@ -2801,28 +2699,28 @@
count=0;
}
} else if(mode==31) {
- motor[TIRE_FR].pwm = 10;
+ motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 10;
+ motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(count>=10000) {
+ if(count>=5000) {
mode=32;
count=0;
}
} else if(mode==32) {
- motor[TIRE_FR].pwm = 5;
+ motor[TIRE_FR].pwm = 10;
motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 5;
+ motor[TIRE_FL].pwm = 10;
motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 5;
+ motor[TIRE_BR].pwm = 10;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 5;
+ motor[TIRE_BL].pwm = 10;
motor[TIRE_BL].dir = BRAKE;
- if(count>=40000) {
+ if(count>=30000) {
mode=33;
count=0;
}