大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 41:e1dd6d97f17d
- Parent:
- 40:7f93984e8ba1
diff -r 7f93984e8ba1 -r e1dd6d97f17d System/Process/Process.cpp
--- a/System/Process/Process.cpp Sat Sep 28 13:15:16 2019 +0000
+++ b/System/Process/Process.cpp Sun Sep 29 07:28:05 2019 +0000
@@ -182,6 +182,8 @@
bool mtc=false;
bool mtc2 = false;
+bool start_flag = true;
+
int mode =1;
int cross=0;//十字数える用
int cros=0;
@@ -311,6 +313,75 @@
for(int i = 0; i < 8; i++) {
g[i] = LineHub::GetPara(i);
}
+ if(ThSW) {
+ Twsh=1;
+ } 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;
+ }
+ }
+ if(DWSW||DOSW) {
+ if(DWSW) {
+ zyouge=true;
+ }
+ if(DOSW) {
+ if(DOLS) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ } else {
+ motor[MOTOR_0].pwm = 50;
+ motor[MOTOR_0].dir = BACK;
+ }
+ }
+ } else if(zyouge==false){
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ if(zyouge) {
+ if(Twsh==1) {
+ motor[MOTOR_0].pwm = 220;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ zyouge=false;
+ }
+ }
+ if(Twsh==2) {
+ motor[MOTOR_0].pwm = 230;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS2) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ zyouge=false;
+ }
+ }
+ }
+ if(AIRSW) {
+ if(SW_flag==false) {
+ if(Air_flag==false) {
+ solenoid.solenoid3 = SOLENOID_ON;
+ Air_flag=true;
+ SW_flag=true;
+ } else {
+ solenoid.solenoid3 = SOLENOID_OFF;
+ Air_flag=false;
+ SW_flag=true;
+ }
+ }
+ } else {
+ SW_flag=false;
+ }
//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]);
@@ -319,7 +390,7 @@
#endif
#ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) { //& start_flag
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
@@ -359,6 +430,7 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
+ ///*
mode=1;
LedMode(1);
if(ThSW) {
@@ -368,6 +440,8 @@
}
if(StertSW) {
+ //start_flag == false;
+ //lock = false;
if(ZoneSW) {
mode=1;
current = 2;
@@ -427,6 +501,7 @@
zyougedo=false;
}
}
+ //*/
}
#endif
@@ -434,6 +509,8 @@
#if USE_PROCESS_NUM>1
static void Process1()
{
+ start_flag = true;
+
mtc=false;
mtc2 = false;
LedMode(2);
@@ -2339,16 +2416,16 @@
LedMode(5);
count++;
if(mode==1) {//スタートゾーンから白線検知
- fast=40;
+ fast=50;
normal=30;
slow=20;
- motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].pwm = 35;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].pwm = 35;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 40;
+ motor[TIRE_BR].pwm = 35;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].pwm = 32;
motor[TIRE_BL].dir = BACK;
if(g[2]==0) {
count=100000;
@@ -2356,7 +2433,7 @@
mode=3;
}
} else if(mode==3) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FR].pwm = 27;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
@@ -2528,9 +2605,6 @@
count=0;
cross=0;
mode=20;
- fast=40;
- normal=30;
- slow=20;
}
} else if(mode==20) {
@@ -2562,7 +2636,7 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- if(count>80000) {
+ if(count>50000) {
cross++;
count=0;
}
@@ -2575,6 +2649,7 @@
if(cross==2) {
mode=21;
cross=0;
+ count=0;
}
break;
case 0:
@@ -2690,6 +2765,18 @@
}
} else if(mode==21) {
motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BRAKE;
+ 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;
@@ -2700,9 +2787,8 @@
if(g[2]!=98) {
mode=30;
}
-
} else if(mode==30) {
- motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].pwm = 64;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
@@ -2712,167 +2798,43 @@
motor[TIRE_BL].dir = BACK;
if(g[1]==98) {
mode=31;
+ count=0;
}
- //ゆっくりモードに入れなかった時の保険
- /*
- 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(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 = 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 = 20;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- 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 = BACK;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FOR;
- 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 = 20;
- 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 = 20;
- 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_FL].pwm += 5;
- mtc=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_FR].pwm = 0;
- mtc=false;
- break;
- case 1:
- motor[TIRE_FR].pwm += 5;
- mtc=false;
- break;
- case 3:
- motor[TIRE_FR].pwm += 10;
- mtc=false;
- break;
- case 2:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc=false;
- break;
- }
-
- }
- */
} else if(mode==31) {
- 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;
- if(g[3]==0) {
+ motor[TIRE_FR].pwm = 10;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 10;
+ motor[TIRE_BL].dir = FREE;
+ if(count>=10000) {
+ mode=32;
count=0;
- mode=32;
}
} else if(mode==32) {
- 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;
+ motor[TIRE_FR].pwm = 5;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 5;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 5;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 5;
+ motor[TIRE_BL].dir = BRAKE;
+ if(count>=40000) {
+ mode=33;
+ count=0;
+ }
+ } else if(mode==33) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
if(g[3]==0) {
count=0;
if(Twsh==2) {
@@ -2881,8 +2843,7 @@
mode=40;
}
} else if(mode==40) {
-
- if(LIF) {
+ if(LIF||LIB) {
mode=42;
}
@@ -3033,6 +2994,9 @@
}
}
} else if(mode==41) {
+ if(LIF||LIB) {
+ mode=42;
+ }
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
@@ -3049,15 +3013,15 @@
slow=40;
}
} else if(mode==42) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
+ 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 = 30;
- motor[TIRE_BL].dir = BACK;
- if(g[1]==0) {
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(g[0]==0) {
count=0;
mode=70;
normal=60;
@@ -3073,7 +3037,7 @@
mode=100;
count=0;
}
- if(cross==3) {
+ if(cross==2) {
cross=0;
mode=100;
}