大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 44:c208a908583c
- Parent:
- 43:677f7446612e
--- a/System/Process/Process.cpp Wed Oct 02 01:53:59 2019 +0000
+++ b/System/Process/Process.cpp Thu Oct 03 06:22:38 2019 +0000
@@ -154,6 +154,7 @@
bool mtc=false;
bool mtc2 = false;
bool emcs=false;
+bool kiri=true;
bool start_flag = true;
@@ -338,7 +339,7 @@
lock = false;
if(ZoneSW) {
mode=1;
- current = 2;
+ current = 6;
} else {
mode=1;
current = 4;
@@ -620,198 +621,7 @@
// 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]);
LedMode(3);
count++;
- if(mode==1) {//スタートゾーンから白線検知
- fast=40;
- normal=30;
- slow=20;
- 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;
- if(g[2]==0) {
- count=100000;
- cross=0;
- mode=3;
- }
- } else if(mode==3) { //横ライントレースから縦ライントレースへ
- 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=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[0]) {
- 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;
- 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 = 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[0]!=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==11) {
- 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[2]==0) {
- count=0;
- cross=0;
- mode=20;
- }
- } else if(mode==20) {
+ if(mode==0||mode==1) {
if(Twsh==2) {
solenoid.solenoid2 = SOLENOID_ON;
@@ -830,7 +640,11 @@
// count=0;
// cros=0;
}//ゆっくりモードに入れなかった時の保険
-
+ if(g[0]==0) {
+ fast=35;
+ normal=20;
+ slow=15;
+ }
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -841,7 +655,7 @@
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- if(count>50000) {
+ if(count>30000) {
cross++;
count=0;
}
@@ -850,6 +664,9 @@
mode=22;
count=0;
cross=0;
+ fast=40;
+ normal=30;
+ slow=20;
}
break;
case 0:
@@ -977,17 +794,17 @@
count=0;
}
}
- if(YELLOW) {
+ if(YELLOW==true) {
if(mode==22) {
- motor[TIRE_FR].pwm = 10;
+ motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 10;
+ motor[TIRE_FL].pwm = 100;
motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].pwm = 10;
+ motor[TIRE_BR].pwm = 100;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].pwm = 10;
+ motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BRAKE;
- if(count<=100000) {
+ if(count>=10000) {
mode=23;
}
} else if(mode==23) {
@@ -999,11 +816,24 @@
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;
+ 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;
+ mode=25;
}
- } else if(mode==24) {
+ } else if(mode==25) {
motor[TIRE_FR].pwm = 10;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 10;
@@ -1012,19 +842,35 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 10;
motor[TIRE_BL].dir = BRAKE;
- if(count<=100000) {
- mode=25;
+ if(count>=10000) {
+ mode=26;
+ count=0;
}
- } else if(mode==25) {
- motor[TIRE_FR].pwm = 15;
+ } else if(mode==26) {
+ motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 15;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 15;
+ 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 = 15;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
- if(count<=100000) {
+ //どれくらい回転するかって値
+ if(count>=2500) {
+ count=0;
mode=30;
}
} else if(mode==24) {
@@ -1036,8 +882,9 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 10;
motor[TIRE_BL].dir = BRAKE;
- if(count<=100000) {
+ if(count>=10000) {
mode=30;
+ count=0;
}
}
} else {
@@ -1057,13 +904,13 @@
}
}
if(mode==30) {
- motor[TIRE_FR].pwm = 60;
+ motor[TIRE_FR].pwm = 59;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 60;
+ motor[TIRE_FL].pwm = 62;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 60;
+ motor[TIRE_BR].pwm = 59;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = 60;
+ motor[TIRE_BL].pwm = 62;
motor[TIRE_BL].dir = FOR;
if(g[0]==98) {
mode=31;
@@ -1096,13 +943,13 @@
count=0;
}
} else if(mode==33) {
- motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FOR;
if(g[3]==0) {
count=0;
@@ -1144,7 +991,7 @@
count=0;
}
mtc2=true;
- if(cross==1) {
+ if(cross==2) {
mode=41;
count=0;
cross=0;
@@ -2248,7 +2095,855 @@
#if USE_PROCESS_NUM>6
static void Process6()
{
+// 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]);
+ LedMode(3);
+ count++;
+ if(mode==1) {//スタートゾーンから白線検知
+ fast=40;
+ normal=30;
+ slow=20;
+ 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;
+ if(g[2]==0) {
+ count=100000;
+ cross=0;
+ mode=3;
+ }
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ 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=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[0]) {
+ 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;
+ 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 = 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[0]!=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==11) {
+ 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[2]==0) {
+ count=0;
+ cross=0;
+ mode=20;
+ }
+ } else if(mode==20) {
+
+ if(Twsh==2) {
+ solenoid.solenoid2 = SOLENOID_ON;
+ }
+ 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;
+ }//ゆっくりモードに入れなかった時の保険
+ if(g[0]==0) {
+ fast=35;
+ normal=20;
+ slow=15;
+ }
+ 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>30000) {
+ cross++;
+ count=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=22;
+ count=0;
+ cross=0;
+ fast=40;
+ normal=30;
+ slow=20;
+ }
+ 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 = 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[2]==98&&count>=10000) {
+ mode=22;
+ count=0;
+ }
+ }
+ 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;
+ 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=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==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>=3300) {
+ count=0;
+ 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>=10000) {
+ mode=30;
+ count=0;
+ }
+ }
+ } 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;
+ }
+ }
+ }
+ if(mode==30) {
+ motor[TIRE_FR].pwm = 59;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm = 62;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 59;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 62;
+ motor[TIRE_BL].dir = FOR;
+ if(g[0]==98) {
+ mode=31;
+ count=0;
+ }
+ } else if(mode==31) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].dir = FREE;
+ motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].dir = FREE;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(count>=5000) {
+ count=0;
+ mode=32;
+ }
+ } else if(mode==32) {
+ 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>=40000) {
+ mode=33;
+ count=0;
+ }
+ } else if(mode==33) {
+ motor[TIRE_FR].pwm = 0;
+ 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 = 0;
+ 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(LIB) {
+ 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_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==41) {
+ if(LIF||LIB) {
+ mode=42;
+ }
+ 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;
+ if(g[1]==0) {
+ count=0;
+ mode=70;
+ }
+ } else if(mode==42) {
+ motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==0) {
+ count=0;
+ mode=70;
+ }
+ } else if(mode==70) {
+ switch(g[1]) {
+ case 99:
+ if(count>=100000) {
+ mode=100;
+ }
+ break;
+ 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;
+ break;
+ case 0:
+ motor[TIRE_FR].pwm = 80;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 80;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 80;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 80;
+ 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==100) {
+ motor[TIRE_FR].pwm = 20;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 23;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BL].dir = BACK;
+ } else {
+ }
}
#endif