大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 45:878f8f385635
- Parent:
- 44:c208a908583c
- Child:
- 46:4a8bca4cc1fd
--- a/System/Process/Process.cpp Thu Oct 03 06:22:38 2019 +0000
+++ b/System/Process/Process.cpp Thu Oct 03 11:09:31 2019 +0000
@@ -154,7 +154,7 @@
bool mtc=false;
bool mtc2 = false;
bool emcs=false;
-bool kiri=true;
+bool kiri;
bool start_flag = true;
@@ -334,7 +334,7 @@
Twsh=2;
}
- if(StertSW && start_flag) {
+ if(StertSW && start_flag == true) {
start_flag = false;
lock = false;
if(ZoneSW) {
@@ -364,7 +364,7 @@
}
if(zyouge) {
if(Twsh==1) {
- motor[MOTOR_0].pwm = 220;
+ motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = FOR;
if(UPLS) {
motor[MOTOR_0].pwm = 100;
@@ -373,7 +373,7 @@
}
}
if(Twsh==2) {
- motor[MOTOR_0].pwm = 230;
+ motor[MOTOR_0].pwm = 235;
motor[MOTOR_0].dir = FOR;
if(UPLS2) {
motor[MOTOR_0].pwm = 100;
@@ -645,6 +645,7 @@
normal=20;
slow=15;
}
+
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1291,16 +1292,16 @@
LedMode(4);
count++;
if(mode==1) {//スタートゾーンから白線検知
- fast=50;
+ fast=40;
normal=30;
slow=20;
- motor[TIRE_FR].pwm = 35;
+ motor[TIRE_FR].pwm = 27;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 35;
+ motor[TIRE_FL].pwm = 33;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 35;
+ motor[TIRE_BR].pwm = 33;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 35;
+ motor[TIRE_BL].pwm = 27;
motor[TIRE_BL].dir = BACK;
if(g[2]==0) {
count=100000;
@@ -1316,7 +1317,7 @@
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
- if(g[0]==0) {
+ if(g[1]==0) {
mode=10;
count=100000;
}
@@ -1500,7 +1501,11 @@
//count=0;
//cros=0;
}//ゆっくりモードに入れなかった時の保険
-
+ if(g[1]==0) {
+ fast=35;
+ normal=20;
+ slow=15;
+ }
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1516,15 +1521,13 @@
count=0;
}
mtc2=true;
- if(cross==1) {
- fast=30;
- normal=20;
- slow=20;
- }
if(cross==2) {
- mode=21;
+ mode=22;
cross=0;
count=0;
+ fast=40;
+ normal=30;
+ slow=20;
}
break;
case 0:
@@ -1640,20 +1643,46 @@
}
} else if(mode==21) {
- motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BRAKE;
if(count<=100000) {
mode=22;
}
}
- if(YELLOW) {
+ kiri=true;
+ if(kiri) {
if(mode==22) {
+ motor[TIRE_FR].pwm = 100;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].pwm = 100;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 100;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 100;
+ motor[TIRE_BL].dir = BRAKE;
+ if(count>=10000) {
+ mode=23;
+ }
+ } else if(mode==23) {
+ 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=24;
+ count=0;
+ }
+ } else if(mode==24) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
@@ -1663,35 +1692,77 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
if(g[2]!=98) {
- mode=23;
+ count=0;
+ mode=25;
+ }
+ } else if(mode==25) {
+ 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>=10000) {
+ mode=26;
+ count=0;
}
- }
- } 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==26) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BACK;
+ if(count>=5000) {
+ mode=27;
+ count=0;
+ }
+ } else if(mode==27) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = FOR;
+ //どれくらい回転するかって値
+ if(count>=4000) {
+ count=0;
+ mode=28;
+ }
+ } else if(mode==28) {
+ 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>=10000) {
+ mode=30;
+ count=0;
+ }
}
} 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) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(g[0]==0&&count>=10000) {
+ mode=30;
count=0;
- cross=0;
- mode=30;
+ cros=0;
}
}
}
@@ -1702,7 +1773,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].pwm = 57;
motor[TIRE_BL].dir = BACK;
if(g[1]==98) {
mode=31;
@@ -1735,15 +1806,15 @@
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) {
+ 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;
+ if(g[3]==0&&count>=20000) {
count=0;
if(Twsh==2) {
solenoid.solenoid2 = SOLENOID_OFF;
@@ -1751,24 +1822,9 @@
mode=40;
}
} else if(mode==40) {
- if(LIF||LIB) {
+ if(LIB) {
mode=42;
}
-
- if(counts==false&&g[0]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[0]==99) {
- counts=false;
- }
-
- if(cros==2) {
- // mode=41;
- //count=0;
- //cros=0;
- }//ゆっくりモードに入れなかった時の保険
-
switch(g[3]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1779,12 +1835,12 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(count>20000) {
+ if(count>=10000) {
cross++;
count=0;
}
mtc2=true;
- if(cross==1) {
+ if(cross==2) {
mode=41;
count=0;
cross=0;
@@ -1917,23 +1973,20 @@
count=0;
mode=70;
normal=60;
- fast=80;
+ fast=70;
slow=40;
}
} else if(mode==42) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[0]==0) {
count=0;
- mode=70;
- normal=60;
- fast=80;
slow=40;
}
@@ -2079,9 +2132,6 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
- if(StertSW) {
- SW_flag = true;
- }
}
}
#endif
@@ -2537,7 +2587,7 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
//どれくらい回転するかって値
- if(count>=3300) {
+ if(count>=100) {
count=0;
mode=30;
}
@@ -2565,8 +2615,9 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(g[0]==0) {
+ if(g[0]==0&&count>=10000) {
mode=30;
+ count=0;
cros=0;
}
}
@@ -2619,7 +2670,7 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FOR;
- if(g[3]==0) {
+ if(g[3]==0&&count>=20000) {
count=0;
if(Twsh==2) {
solenoid.solenoid2 = SOLENOID_OFF;
@@ -2791,6 +2842,9 @@
if(g[1]==0) {
count=0;
mode=70;
+ normal=60;
+ fast=70;
+ slow=40;
}
} else if(mode==42) {
motor[TIRE_FR].pwm = 20;