にいむら にいむら
aa
Dependencies: mbed TrapezoidControl QEI
Revision 36:788274604650, committed 2019-09-25
- Comitter:
- yabahiro
- Date:
- Wed Sep 25 11:03:03 2019 +0000
- Parent:
- 35:94f026ab4d30
- Commit message:
- aa;
Changed in this revision
--- a/Input/Potentiometer/Potentiometer.h Mon Sep 23 08:58:24 2019 +0000
+++ b/Input/Potentiometer/Potentiometer.h Wed Sep 25 11:03:03 2019 +0000
@@ -4,10 +4,10 @@
#include "mbed.h"
namespace POTENTIOMETER {
- #define ADC0_PIN PC_3
- #define ADC1_PIN PC_0
- #define ADC2_PIN PC_1
- #define ADC3_PIN PC_0
+ #define ADC0_PIN PC_2
+ #define ADC1_PIN PC_3
+ #define ADC2_PIN PC_0
+ #define ADC3_PIN PC_1
#define ADC4_PIN PB_0
extern DigitalOut dio[];
--- a/System/Process/Process.cpp Mon Sep 23 08:58:24 2019 +0000
+++ b/System/Process/Process.cpp Wed Sep 25 11:03:03 2019 +0000
@@ -106,33 +106,59 @@
void LedMode(int led)
{
+ switch(led) {
+ case 1:
+ POTENTIOMETER::dio[0]= 1;
+ POTENTIOMETER::dio[1]= 0;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
+ break;
+ case 2:
+ POTENTIOMETER::dio[0]= 0;
+ POTENTIOMETER::dio[1]= 1;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
+ break;
+ case 3:
+ POTENTIOMETER::dio[0]= 1;
+ POTENTIOMETER::dio[1]= 1;
+ POTENTIOMETER::dio[2]= 0;
+ POTENTIOMETER::dio[3]= 0;
+ break;
+ case 4:
+ POTENTIOMETER::dio[0]= 0;
+ POTENTIOMETER::dio[1]= 0;
+ POTENTIOMETER::dio[2]= 1;
+ POTENTIOMETER::dio[3]= 0;
+ break;
+ }
/*
if(led/8>=1) {
- POTENTIOMETER::dio[0]= 1;
+ POTENTIOMETER::dio[3]= 1;
led=led%8;
} else {
- POTENTIOMETER::dio[0]= 0;
+ POTENTIOMETER::dio[3]= 0;
}
if(led/4>=1) {
+ POTENTIOMETER::dio[2]= 1;
+ led=led%4;
+ } else {
+ POTENTIOMETER::dio[2]= 0;
+ }
+
+ if(led/2>=1) {
POTENTIOMETER::dio[1]= 1;
- led=led%4;
+ led=led%2;
} else {
POTENTIOMETER::dio[1]= 0;
}
- if(led/2>=1) {
- POTENTIOMETER::dio[2]= 1;
- led=led%2;
+ if(led/1>=1) {
+ POTENTIOMETER::dio[0]= 1;
+ led=0;
} else {
- POTENTIOMETER::dio[2]= 0;
- }
-
- if(led/1>=1) {
- POTENTIOMETER::dio[3]= 1;
- led=led%1;
- } else {
- POTENTIOMETER::dio[3]= 0;
+ POTENTIOMETER::dio[0]= 0;
}
*/
}
@@ -152,6 +178,8 @@
bool zone;
bool through=false;
bool counts=false;
+bool mtc=false;
+bool mtc2 = false;
int mode =1;
int cross=0;//十字数える用
@@ -283,7 +311,7 @@
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]);
+ //printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%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();
@@ -328,39 +356,29 @@
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
-
static void Process0()
{
+ mode=1;
+ LedMode(1);
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==false) {
- if(ThSW) {
- Twsh=1;
- } else {
- Twsh=2;
- }
+ if(ZoneSW) {
mode=1;
current = 2;
} else {
- if(ThSW) {
- Twsh=1;
- } else {
- Twsh=2;
- }
mode=1;
- current = 3;
+ current = 4;
}
}
if(DWSW) {
zyouge=true;
+ } else {
+ zyouge=false;
}
if(AIRSW) {
if(SW_flag==false) {
@@ -405,12 +423,9 @@
#if USE_PROCESS_NUM>1
static void Process1()
{
- POTENTIOMETER::dio[0]= 0;
- POTENTIOMETER::dio[1]= 1;
- POTENTIOMETER::dio[2]= 0;
- POTENTIOMETER::dio[3]= 0;
- //LedMode(2);
- count=100000;
+ mtc=false;
+ mtc2 = false;
+ LedMode(2);
mode=1;
cross=0;
Twsh=1;
@@ -507,17 +522,9 @@
#endif
#if USE_PROCESS_NUM>2
-bool mtc=false;
-//追加
-bool mtc2 = false;
-
static void Process2()
{
- POTENTIOMETER::dio[0]= 1;
- POTENTIOMETER::dio[1]= 1;
- POTENTIOMETER::dio[2]= 0;
- POTENTIOMETER::dio[3]= 0;
- //LedMode(3);
+ 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) {//スタートゾーンから白線検知
@@ -1040,7 +1047,7 @@
}
if(cros==2) {
- // mode=41;
+ // mode=41;
//count=0;
//cros=0;
}//ゆっくりモードに入れなかった時の保険
@@ -1339,16 +1346,19 @@
motor[TIRE_BL].dir = BRAKE;
}
}
-
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
- POTENTIOMETER::dio[0]= 0;
- POTENTIOMETER::dio[1]= 0;
- POTENTIOMETER::dio[2]= 1;
- POTENTIOMETER::dio[3]= 0;
+}
+#endif
+
+#if USE_PROCESS_NUM>4
+static void Process4()
+{
+ LedMode(4);
+ count++;
if(mode==1) {//スタートゾーンから白線検知
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
@@ -1361,22 +1371,36 @@
if(g[2]==0) {
count=100000;
cross=0;
- mode=2;
+ mode=3;
}
- } else if(mode==2) { //横ライントレースから縦ライントレースへ
- motor[TIRE_FR].pwm = 0;
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].pwm = normal;
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;
- mode=10;
- }
- } else if(g[1]!=99&&mode==10) { //縦ライントレース
+ cros=0;
+ }//ゆっくりモードに入れなかった時の保険
switch(g[1]) {
case 98:
motor[TIRE_FR].pwm = normal;
@@ -1387,14 +1411,15 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
- 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:
@@ -1475,8 +1500,8 @@
mtc=true;
break;
}
- if(g[1]!=98&&mtc==true) {
- switch(g[0]) {
+ if(g[0]!=98&&mtc==true) {
+ switch(g[1]) {
case 0:
mtc=false;
break;
@@ -1509,19 +1534,35 @@
}
}
} else if(mode==11) {
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
+ 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[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;
@@ -1532,12 +1573,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;
@@ -1555,13 +1596,13 @@
mtc2=true;
break;
case 255:
- motor[TIRE_FR].pwm = slow;
+ 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 = slow;
+ motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
@@ -1655,28 +1696,40 @@
}
}
} else if(mode==21) {
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
- count=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[1]) {
case 98:
- motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
if(count>100000) {
cross++;
@@ -1684,71 +1737,74 @@
}
mtc=true;
if(cross==1) {
+ 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 = FOR;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 255:
- motor[TIRE_FR].pwm = fast;
+ motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = fast;
+ motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 253:
- motor[TIRE_FR].pwm = slow;
+ 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 = slow;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 254:
- motor[TIRE_FR].pwm = normal;
+ 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 = normal;
+ motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 1:
- motor[TIRE_FR].pwm = slow;
+ motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast;
+ motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = fast;
+ motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = slow;
+ 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 = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = slow;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -1757,9 +1813,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 = BACK;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -1800,19 +1856,34 @@
}
}
} else if(mode==31) {
- motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 0;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 0;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[3]==0) {
count=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;
@@ -1823,12 +1894,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;
@@ -1946,310 +2017,20 @@
}
}
} else if(mode==41) {
- motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
- count=0;
- mode=50;
- }
- } else if(mode==50) {
- 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;
- 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 = 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==51) {
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(g[3]==0) {
- count=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 = BACK;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FREE;
- motor[TIRE_BL].pwm = 0;
- motor[TIRE_BL].dir = FOR;
- if(g[0]==0) {
+ if(g[1]==0) {
count=0;
mode=70;
}
- } else if(mode==70) {
+
+ } else if(mode==70)
switch(g[0]) {
case 99:
if(count>=100000) {
@@ -2344,49 +2125,49 @@
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;
- }
+ if(g[1]!=98&&mtc==true) {
+ switch(g[0]) {
+ 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 = slow;
+ motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 23;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
- } else {
+ }
+ /* else {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 0;
@@ -2396,14 +2177,7 @@
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = BRAKE;
}
-}
-
-#endif
-
-#if USE_PROCESS_NUM>4
-static void Process4()
-{
-
+ */
}
#endif