大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 35:94f026ab4d30
- Parent:
- 34:cef6c124a52d
diff -r cef6c124a52d -r 94f026ab4d30 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sun Sep 22 00:53:45 2019 +0000
+++ b/System/Process/Process.cpp Mon Sep 23 08:58:24 2019 +0000
@@ -104,9 +104,43 @@
else return abs(pwmVal);
}
+void LedMode(int led)
+{
+ /*
+ if(led/8>=1) {
+ POTENTIOMETER::dio[0]= 1;
+ led=led%8;
+ } else {
+ POTENTIOMETER::dio[0]= 0;
+ }
+
+ if(led/4>=1) {
+ POTENTIOMETER::dio[1]= 1;
+ led=led%4;
+ } else {
+ POTENTIOMETER::dio[1]= 0;
+ }
+
+ if(led/2>=1) {
+ POTENTIOMETER::dio[2]= 1;
+ led=led%2;
+ } else {
+ POTENTIOMETER::dio[2]= 0;
+ }
+
+ if(led/1>=1) {
+ POTENTIOMETER::dio[3]= 1;
+ led=led%1;
+ } else {
+ POTENTIOMETER::dio[3]= 0;
+ }
+ */
+}
int Twsh;
bool UP_flag = false;
+bool SW_flag = false;
bool Air_flag = false;
+bool zyouge;
bool dz1=true;
bool dz1i=false;
bool dz2=true;
@@ -117,9 +151,11 @@
bool dz4i=false;
bool zone;
bool through=false;
+bool counts=false;
int mode =1;
int cross=0;//十字数える用
+int cros=0;
int count=100000;//wait代替え
///*********PWM調整用*********///
@@ -246,9 +282,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
@@ -295,15 +331,17 @@
static void Process0()
{
- for(int i=0; i<=3; i++) {
- if(i==0) {
- POTENTIOMETER::dio[0]= 1;
- } else {
- POTENTIOMETER::dio[i]=0;
- }
+ if(ThSW) {
+ Twsh=1;
+ } else {
+ Twsh=2;
}
+ POTENTIOMETER::dio[0]= 1;
+ POTENTIOMETER::dio[1]= 0;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
if(StertSW) {
- if(ZoneSW) {
+ if(ZoneSW==false) {
if(ThSW) {
Twsh=1;
} else {
@@ -321,41 +359,61 @@
current = 3;
}
}
-
- if(UPSW) {
- UP_flag = true;
+ if(DWSW) {
+ zyouge=true;
+ }
+ 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;
}
- if(UP_flag) {
- motor[MOTOR_0].dir = FOR;
- motor[MOTOR_0].pwm = 100;
-
- if (UPLS) {
- motor[MOTOR_0].dir = BRAKE;
- UP_flag = false;
- Air_flag = true;
+ 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 = 240;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS2) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ zyouge=false;
+ }
}
}
- if(Air_flag) {
- solenoid.solenoid2 = SOLENOID_ON;
- }
+}
-}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
- for(int i=0; i<=3; i++) {
- if(i==1) {
- POTENTIOMETER::dio[1]=1;
- } else {
- POTENTIOMETER::dio[i]=0;
- }
- }
+ POTENTIOMETER::dio[0]= 0;
+ POTENTIOMETER::dio[1]= 1;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
+ //LedMode(2);
count=100000;
mode=1;
cross=0;
+ Twsh=1;
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]);
@@ -369,8 +427,12 @@
if(controller->Button.UP||controller->Button.DOWN) {
if(controller->Button.UP) {
- motor[MOTOR_0].pwm = 110;
+ motor[MOTOR_0].pwm =200;
motor[MOTOR_0].dir = FOR;
+ if(UPLS) {
+ motor[MOTOR_0].pwm = 0;
+ motor[MOTOR_0].dir = BRAKE;
+ }
}
if(controller->Button.DOWN) {
motor[MOTOR_0].pwm = 50;
@@ -380,20 +442,6 @@
motor[MOTOR_0].pwm = 0;
motor[MOTOR_0].dir = BRAKE;
}
- if(controller->Button.X) {
- if(dz1==true) {
- if(dz1i==false) {
- solenoid.solenoid1 = SOLENOID_ON;
- dz1i=true;
- } else {
- solenoid.solenoid1 = SOLENOID_OFF;
- dz1i=false;
- }
- dz1=false;
- }
- } else {
- dz1=true;
- }
if(controller->Button.Y) {
if(dz2==true) {
@@ -465,13 +513,11 @@
static void Process2()
{
- for(int i=0; i<=3; i++) {
- if(i==2) {
- POTENTIOMETER::dio[2]=1;
- } else {
- POTENTIOMETER::dio[i]=0;
- }
- }
+ POTENTIOMETER::dio[0]= 1;
+ POTENTIOMETER::dio[1]= 1;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
+ //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) {//スタートゾーンから白線検知
@@ -486,23 +532,7 @@
if(g[2]==0) {
count=100000;
cross=0;
- mode=2;
- }
- } else if(mode==2) {//横移動
- if(g[2]==0) {
- count=100000;
- cross=0;
mode=3;
- mtc2=true;
- } else {
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = BACK;
}
} else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
@@ -514,28 +544,24 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[0]==0) {
- through=true;
+ mode=10;
+ count=100000;
}
- if(through==true&&g[0]==255) {
- through=false;
- count=0;
- mode=4;
+ } else if(mode==10) { //縦ライントレース
+
+ if(counts==false&&g[2]==0) {
+ cros++;
+ counts=true;
}
- } else if(mode==4) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
+ if(counts==true&&g[2]==99) {
+ counts=false;
+ }
+
+ if(cros==Twsh) {
+ mode=11;
count=0;
- cross=0;
- mode=10;
- }
- } else if(g[0]!=99&&mode==10) { //縦ライントレース
+ cros=0;
+ }//ゆっくりモードに入れなかった時の保険
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -546,14 +572,15 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(count>100000) {
+ mtc=true;
+ if(count>80000) {
cross++;
count=0;
}
- mtc=true;
if(cross==Twsh) {
mode=11;
count=0;
+ cross=0;
}
break;
case 0:
@@ -677,27 +704,26 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[2]==0) {
- through=true;
- }
- if(through==true&&g[2]==255) {
- through=false;
- mode=12;
- }
- } else if(mode==12) {
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = BACK;
- if(g[2]==0) {
count=0;
cross=0;
mode=20;
}
} else if(mode==20) {
+
+ if(counts==false&&g[0]==0) {
+ cros++;
+ counts=true;
+ }
+ if(counts==true&&g[0]==99) {
+ counts=false;
+ }
+
+ if(cros==2) {
+ mode=21;
+ count=0;
+ cros=0;
+ }//ゆっくりモードに入れなかった時の保険
+
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -708,12 +734,12 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- if(count>70000) {
+ if(count>80000) {
cross++;
count=0;
}
mtc2=true;
- if(cross==3) {
+ if(cross==2) {
mode=21;
count=0;
cross=0;
@@ -840,37 +866,31 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[0]==0) {
- through=true;
- }
- if(through==true&&g[0]==255) {
- through=false;
- mode=22;
- }
- } else if(mode==22) {
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
- solenoid.solenoid3 = SOLENOID_ON;
- count=0;
- cross=0;
mode=30;
+ cros=0;
+ zyouge=true;
+ if(Twsh==2) {
+ solenoid.solenoid2 = SOLENOID_ON;
+ }
}
} else if(mode==30) {
+
+ if(counts==false&&g[3]==0) {
+ counts=false;
+ mode=31;
+ count=0;
+ cros=0;
+ }//ゆっくりモードに入れなかった時の保険
+
switch(g[0]) {
case 98:
- motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
if(count>100000) {
cross++;
@@ -878,72 +898,74 @@
}
mtc=true;
if(cross==1) {
- solenoid.solenoid3 = SOLENOID_OFF;
+ if(Twsh==2) {
+ solenoid.solenoid2 = SOLENOID_OFF;
+ }
mode=31;
count=0;
}
break;
case 0:
- motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
- motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
- motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].pwm = 20;
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 = slow;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
- motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].pwm = 30;
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 = normal;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
- motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -952,9 +974,9 @@
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -995,33 +1017,34 @@
}
}
} else if(mode==31) {
- motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].pwm = 30;
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 = 15;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
+ if(g[3]==0) {
count=0;
- mode=32;
- }
- } else if(mode==32) {
- 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(count>5000&&g[3]==0) {
- count=0;
- cross=0;
mode=40;
}
} else if(mode==40) {
+
+ 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;
@@ -1032,12 +1055,12 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(count>30000) {
+ if(count>20000) {
cross++;
count=0;
}
mtc2=true;
- if(cross==1) {
+ if(cross==3) {
mode=41;
count=0;
cross=0;
@@ -1157,347 +1180,17 @@
} else if(mode==41) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=42;
- }
- } else if(mode==42) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=50;
- }
- } else if(mode==50) {
- switch(g[1]) {
- case 98:
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = BACK;
- if(count>100000) {
- cross++;
- count=0;
- }
- mtc=true;
- if(cross==1) {
- mode=51;
- count=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = slow;
- 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 = slow;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
- break;
- case 254:
- 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;
- mtc=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = slow;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
- motor[TIRE_BL].dir = BACK;
- mtc=true;
- break;
- case 3:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- 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 = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- 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==51) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- 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_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(count>5000&&g[3]==0) {
- count=0;
- cross=0;
- mode=60;
- }
- } else if(mode==60) {
- switch(g[3]) {
- case 98:
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = FOR;
- if(count>50000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==2) {
- mode=61;
- count=0;
- cross=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = slow;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- 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 = normal;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
- break;
- case 3:
- motor[TIRE_FR].pwm = slow;
- 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 = slow;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
- break;
- case 2:
- motor[TIRE_FR].pwm = normal;
- 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 = normal;
- motor[TIRE_BL].dir = FOR;
- mtc2=true;
- break;
- }
- if(g[2]!=98&&mtc2==true) {
- switch(g[2]) {
- case 0:
- mtc2=false;
- break;
- case 255:
- motor[TIRE_FL].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FL].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FL].pwm += 20;
- motor[TIRE_BL].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BL].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BL].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BL].pwm += 20;
- motor[TIRE_FL].pwm = 0;
- mtc2=false;
- break;
- }
- }
- } else if(mode==61) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[1]==0) {
- count=0;
- mode=62;
- }
- } else if(mode==62) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- if(count>5000&&g[1]==0) {
+ if(g[1]==0) {
count=0;
mode=70;
}
+
} else if(mode==70) {
switch(g[1]) {
case 99:
@@ -1627,13 +1320,13 @@
}
}
} else if(mode==100) {
- motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 23;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = BACK;
} else {
motor[TIRE_FR].pwm = 0;
@@ -1652,13 +1345,10 @@
#if USE_PROCESS_NUM>3
static void Process3()
{
- for(int i=0; i<=3; i++) {
- if(i==3) {
- POTENTIOMETER::dio[3]=1;
- } else {
- POTENTIOMETER::dio[i]=0;
- }
- }
+ POTENTIOMETER::dio[0]= 0;
+ POTENTIOMETER::dio[1]= 0;
+ POTENTIOMETER::dio[2]= 1;
+ POTENTIOMETER::dio[3]= 0;
if(mode==1) {//スタートゾーンから白線検知
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
@@ -1673,23 +1363,7 @@
cross=0;
mode=2;
}
- } else if(mode==2) {//横移動
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=3;
- mtc2=true;
- } else {
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = BACK;
- }
- } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ } else if(mode==2) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
@@ -1698,24 +1372,10 @@
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = BACK;
- if(count>100000&&g[0]==0) {
+ if(g[0]==0) {
count=0;
mode=10;
}
- } else if(mode==4) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- if(count>30000&&g[0]==0) {
- count=0;
- cross=0;
- mode=10;
- }
} else if(g[1]!=99&&mode==10) { //縦ライントレース
switch(g[1]) {
case 98:
@@ -1851,28 +1511,14 @@
} else if(mode==11) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = BACK;
- if(count>5000&&g[2]==0) {
+ if(g[2]==0) {
count=0;
- mode=12;
- }
- } else if(mode==12) {
- 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(count>10000&&g[2]==0) {
- count=0;
- cross=0;
mode=20;
}
} else if(mode==20) {
@@ -2011,28 +1657,14 @@
} else if(mode==21) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = BACK;
- if(count>2000&&g[1]==0) {
+ if(g[1]==0) {
count=0;
- mode=22;
- }
- } else if(mode==22) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- if(count>5000&&g[1]==0) {
- count=0;
- cross=0;
mode=30;
}
} else if(mode==30) {
@@ -2168,30 +1800,16 @@
}
}
} else if(mode==31) {
- motor[TIRE_FR].pwm = 15;
+ motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FREE;
- if(count>5000&&g[3]==0) {
+ if(g[3]==0) {
count=0;
- mode=32;
- }
- } else if(mode==32) {
- 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(count>5000&&g[3]==0) {
- count=0;
- cross=0;
mode=40;
}
} else if(mode==40) {
@@ -2336,20 +1954,7 @@
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[0]==0) {
- count=0;
- mode=42;
- }
- } else if(mode==42) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- if(count>5000&&g[0]==0) {
+ if(g[0]==0) {
count=0;
mode=50;
}
@@ -2488,28 +2093,14 @@
} else if(mode==51) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[3]==0) {
+ if(g[3]==0) {
count=0;
- mode=52;
- }
- } else if(mode==52) {
- 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(count>5000&&g[3]==0) {
- count=0;
- cross=0;
mode=60;
}
} else if(mode==60) {
@@ -2648,26 +2239,13 @@
} else if(mode==61) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 15;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FOR;
- if(count>5000&&g[0]==0) {
- count=0;
- mode=62;
- }
- } else if(mode==62) {
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- 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 = BACK;
- if(count>5000&&g[0]==0) {
+ if(g[0]==0) {
count=0;
mode=70;
}