大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 43:677f7446612e
- Parent:
- 42:4f9689c0a349
diff -r 4f9689c0a349 -r 677f7446612e System/Process/Process.cpp
--- a/System/Process/Process.cpp Mon Sep 30 11:33:22 2019 +0000
+++ b/System/Process/Process.cpp Wed Oct 02 01:53:59 2019 +0000
@@ -124,6 +124,8 @@
{1,1,1,1}
};
+DigitalIn EMC(PB_0);
+
void LedMode(int num)
{
POTENTIOMETER::dio[0]= selectnum[num][0];
@@ -151,6 +153,7 @@
bool counts=false;
bool mtc=false;
bool mtc2 = false;
+bool emcs=false;
bool start_flag = true;
@@ -280,6 +283,40 @@
SystemProcessInitialize();
while(1) {
+ if(EMC==0) {
+ fast=40;
+ normal=30;
+ slow=20;
+ cross=0;//十字数える用
+ cros=0;
+ count=100000;//wait代替え
+ UP_flag = false;
+ SW_flag = false;
+ Air_flag = false;
+ zyouge=false;
+ zyougedo=false;
+ dz1=true;
+ dz1i=false;
+ dz2=true;
+ dz2i=false;
+ dz3=true;
+ dz3i=false;
+ dz4=true;
+ dz4i=false;
+ through=false;
+ counts=false;
+ mtc=false;
+ mtc2 = false;
+ current = 0;
+ SystemProcessInitialize();
+ if(emcs==false) {
+ AllActuatorReset();
+ emcs=true;
+ }
+ } else {
+ emcs=false;
+ }
+
if(mode==0) {
if(ZoneSW) {
LedMode(1);
@@ -304,7 +341,7 @@
current = 2;
} else {
mode=1;
- current = 5;
+ current = 4;
}
}
if(DWSW||DOSW) {
@@ -359,7 +396,7 @@
} else {
if( SW_flag==true) {
count++;
- if(count>=5000) {
+ if(count>=2000) {
SW_flag=false;
count=0;
}
@@ -390,20 +427,7 @@
//ロック時の処理
}
}
- /*
- //Emergency!
- if(!EMG_0 && !EMG_1 && !EMGflag) {
- buzzer = 0;
- BuzzerTimer.attach(BuzzerTimer_func, 1);
- EMGflag = true;
- LED_DEBUG0 = 1;
- }
- if(EMG_0 && EMG_1 && EMGflag) {
- buzzer = 1;
- BuzzerTimer.detach();
- EMGflag = false;
- }
- */
+
SystemProcessUpdate();
}
}
@@ -486,7 +510,6 @@
}
*/
}
-
#endif
#if USE_PROCESS_NUM>1
@@ -823,8 +846,8 @@
count=0;
}
mtc2=true;
- if(cross==1) {
- mode=21;
+ if(cross==2) {
+ mode=22;
count=0;
cross=0;
}
@@ -941,11 +964,11 @@
}
}
} else if(mode==21) {
- motor[TIRE_FR].pwm = 26;
+ motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 26;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
@@ -953,46 +976,88 @@
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;
+ }
+ if(YELLOW) {
+ if(mode==22) {
+ 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=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) {
+ count=0;
+ mode=24;
+ }
+ } else if(mode==24) {
+ 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=25;
+ }
+ } else if(mode==25) {
+ 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 = FOR;
+ motor[TIRE_BL].pwm = 15;
+ motor[TIRE_BL].dir = FOR;
+ if(count<=100000) {
+ mode=30;
+ }
+ } else if(mode==24) {
+ 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==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==22) {
+ 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) {
+ mode=30;
+ cros=0;
+ }
}
- } else if(mode==24) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- if(g[0]==0) {
- mode=30;
- cros=0;
- }
- } else if(mode==30) {
- motor[TIRE_FR].pwm = 63;
+ }
+ if(mode==30) {
+ motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = FOR;
@@ -1363,14 +1428,6 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = BACK;
} else {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = BRAKE;
}
}
#endif
@@ -1378,7 +1435,6 @@
#if USE_PROCESS_NUM>3
static void Process3()
{
-
}
#endif
@@ -1388,910 +1444,6 @@
LedMode(4);
count++;
if(mode==1) {//スタートゾーンから白線検知
- 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 = 45;
- motor[TIRE_BL].dir = BACK;
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=3;
- }
- } else if(mode==3) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 40;
- 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 = 40;
- motor[TIRE_BL].dir = BACK;
- if(g[0]==0) {
- mode=10;
- count=100000;
- }
- } else if(mode==10) { //縦ライントレース
-
- if(counts==false&&g[2]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[2]==99) {
- counts=false;
- }
-
- if(cros==Twsh) {
- mode=11;
- count=0;
- cros=0;
- }//ゆっくりモードに入れなかった時の保険
- 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;
- mtc=true;
- if(count>80000) {
- cross++;
- count=0;
- }
- if(cross==Twsh) {
- mode=11;
- count=0;
- cross=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[0]!=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==11) {
- 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[2]==0) {
- count=0;
- cross=0;
- mode=20;
- fast=40;
- normal=30;
- slow=20;
- }
- } else if(mode==20) {
-
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_ON;
- }
-
- if(counts==false&&g[1]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[1]==99) {
- counts=false;
- }
-
- if(cros==2) {
- //mode=21;
- //count=0;
- //cros=0;
- }//ゆっくりモードに入れなかった時の保険
-
- switch(g[2]) {
- case 98:
- 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;
- if(count>80000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==2) {
- mode=21;
- count=0;
- cross=0;
- }
- break;
- case 0:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 253:
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow;
- 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 = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc2=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = BACK;
- mtc2=true;
- break;
- case 3:
- 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;
- mtc2=true;
- break;
- case 2:
- 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;
- mtc2=true;
- break;
- }
- 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==21) {
- 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) {
- mode=30;
- cros=0;
- zyouge=true;
- fast=80;
- normal=60;
- slow=40;
- }
- } else if(mode==30) {
- 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 = 50;
- motor[TIRE_BL].dir = BACK;
- if(g[1]==98) {
- mode=31;
- }
- if(g[3]==0) {
- mode=32;
- count=0;
- cros=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) {
- 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;
- if(g[3]==0) {
- count=0;
- if(Twsh==2) {
- solenoid.solenoid2 = SOLENOID_OFF;
- }
- mode=40;
- }
- } else if(mode==40) {
-
- if(LIF) {
- mode=42;
- }
-
- if(counts==false&&g[0]==0) {
- cros++;
- counts=true;
- }
- if(counts==true&&g[0]==99) {
- counts=false;
- }
-
- if(cros==1) {
- // mode=41;
- //count=0;
- //cros=0;
- }//ゆっくりモードに入れなかった時の保険
-
- 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>20000) {
- cross++;
- count=0;
- }
- mtc2=true;
- if(cross==2) {
- mode=41;
- 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_FR].pwm += 5;
- mtc2=false;
- break;
- case 253:
- motor[TIRE_FR].pwm += 10;
- mtc2=false;
- break;
- case 254:
- motor[TIRE_FR].pwm += 20;
- motor[TIRE_BR].pwm = 0;
- mtc2=false;
- break;
- case 1:
- motor[TIRE_BR].pwm += 5;
- mtc2=false;
- break;
- case 3:
- motor[TIRE_BR].pwm += 10;
- mtc2=false;
- break;
- case 2:
- motor[TIRE_BR].pwm += 20;
- motor[TIRE_FR].pwm = 0;
- mtc2=false;
- break;
- }
- }
- } else if(mode==41) {
- 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 = 30;
- motor[TIRE_BL].dir = FOR;
- if(g[1]==0) {
- count=0;
- mode=70;
- normal=60;
- fast=80;
- slow=40;
- }
- } else if(mode==70)
- switch(g[0]) {
- case 99:
- if(count>=100000) {
- cross++;
- mode=100;
- count=0;
- }
- if(cross==3) {
- cross=0;
- mode=100;
- }
- break;
- case 98:
- motor[TIRE_FR].pwm = normal;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- break;
- case 0:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 255:
- motor[TIRE_FR].pwm = fast;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast;
- motor[TIRE_BL].dir = FOR;
- mtc=true;
- break;
- case 253:
- 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;
- mtc=true;
- break;
- case 254:
- 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;
- mtc=true;
- break;
- case 1:
- motor[TIRE_FR].pwm = slow;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow;
- 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].dir = FOR;
- motor[TIRE_BR].pwm = slow;
- 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 = normal;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FREE;
- mtc=true;
- break;
- }
- if(g[1]!=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==100) {
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 23;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FOR;
- if(StertSW) {
- SW_flag = true;
- }
- if(SW_flag ==true) {
- 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;
- fast=60;
- normal=40;
- slow=20;
- mode =1;
- cross=0;//十字数える用
- cros=0;
- count=100000;//wait代替え
- UP_flag = false;
- SW_flag = false;
- Air_flag = false;
- zyouge=false;
- zyougedo=false;
- dz1=true;
- dz1i=false;
- dz2=true;
- dz2i=false;
- dz3=true;
- dz3i=false;
- dz4=true;
- dz4i=false;
- through=false;
- counts=false;
- mtc=false;
- mtc2 = false;
- current = 0;
- if(StertSW==false) {
- SW_flag = false;
- }
- }
- }
-
-
-}
-#endif
-
-#if USE_PROCESS_NUM>5
-static void Process5()
-{
- LedMode(4);
- count++;
- if(mode==1) {//スタートゾーンから白線検知
fast=50;
normal=30;
slow=20;
@@ -2301,7 +1453,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 35;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 32;
+ motor[TIRE_BL].pwm = 35;
motor[TIRE_BL].dir = BACK;
if(g[2]==0) {
count=100000;
@@ -2309,13 +1461,13 @@
mode=3;
}
} else if(mode==3) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 27;
+ 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 = 25;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
if(g[0]==0) {
mode=10;
@@ -2664,10 +1816,21 @@
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
if(g[2]!=98) {
- mode=30;
+ 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==22) {
motor[TIRE_FR].pwm = normal;
@@ -2686,7 +1849,7 @@
}
}
if(mode==30) {
- motor[TIRE_FR].pwm = 64;
+ motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
@@ -3072,48 +2235,13 @@
if(StertSW) {
SW_flag = true;
}
- if(SW_flag ==true) {
- 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;
- fast=60;
- normal=40;
- slow=20;
- mode =1;
- cross=0;//十字数える用
- cros=0;
- count=100000;//wait代替え
- UP_flag = false;
- SW_flag = false;
- Air_flag = false;
- zyouge=false;
- zyougedo=false;
- dz1=true;
- dz1i=false;
- dz2=true;
- dz2i=false;
- dz3=true;
- dz3i=false;
- dz4=true;
- dz4i=false;
- through=false;
- counts=false;
- mtc=false;
- mtc2 = false;
- current = 0;
- if(StertSW==false) {
- SW_flag = false;
- }
- }
}
-
+}
+#endif
-
+#if USE_PROCESS_NUM>5
+static void Process5()
+{
}
#endif