大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 40:7f93984e8ba1
- Parent:
- 39:f89166d7411c
- Child:
- 41:e1dd6d97f17d
diff -r f89166d7411c -r 7f93984e8ba1 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sat Sep 28 02:49:10 2019 +0000
+++ b/System/Process/Process.cpp Sat Sep 28 13:15:16 2019 +0000
@@ -188,9 +188,9 @@
int count=100000;//wait代替え
///*********PWM調整用*********///
-int fast =80;
-int normal = 60;
-int slow = 40;
+int fast =60;
+int normal = 40;
+int slow = 20;
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
@@ -373,7 +373,7 @@
current = 2;
} else {
mode=1;
- current = 4;
+ current = 5;
}
}
if(DWSW) {
@@ -1441,7 +1441,7 @@
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 35;
+ motor[TIRE_BL].pwm = 45;
motor[TIRE_BL].dir = BACK;
if(g[2]==0) {
count=100000;
@@ -1990,7 +1990,7 @@
counts=false;
}
- if(cros==2) {
+ if(cros==1) {
// mode=41;
//count=0;
//cros=0;
@@ -2289,50 +2289,968 @@
if(StertSW) {
SW_flag = true;
}
- if(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) {
+ 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(5);
+ count++;
+ if(mode==1) {//スタートゾーンから白線検知
+ fast=40;
+ normal=30;
+ slow=20;
+ 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 = 40;
+ motor[TIRE_BL].dir = BACK;
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=3;
+ }
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ motor[TIRE_FR].pwm = 25;
+ 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].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==1) {
+ fast=30;
+ normal=20;
+ slow=20;
+ }
+ if(cross==2) {
+ mode=21;
+ 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 = 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) {
+ 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;
+ if(g[1]==98) {
+ mode=31;
+ }
+ //ゆっくりモードに入れなかった時の保険
+ /*
+ 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==2) {
+ // 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==1) {
+ 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==42) {
+ motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].dir = FOR;
+ 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) {
+ 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