大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 42:980af34f6fe3
- Parent:
- 41:99a1158f3eca
- Child:
- 43:1bb4fbe7d4c6
--- a/System/Process/Process.cpp Fri Nov 08 13:57:55 2019 +0000
+++ b/System/Process/Process.cpp Sat Nov 09 06:17:24 2019 +0000
@@ -57,14 +57,8 @@
const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
-uint8_t SetStatus(int);
-uint8_t SetPWM(int);
-
-uint8_t Setpwm(int pwmVal)
-{
- if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
- else return abs(pwmVal);
-}
+uint8_t SetStatus(int pwmVal);
+uint8_t SetPWM(int pwmVal);
DigitalIn EMC(PC_13); //非常停止確認用
@@ -74,8 +68,6 @@
bool Air_flag = false;
bool zyouge;
bool zyougedo;
-bool dz1=true;
-bool dz1i=false;
bool dz2=true;
bool dz2i=false;
bool dz3=true;
@@ -100,21 +92,15 @@
///*********PWM調整用*********///
-int fast =60;
+int fast = 60;
int normal = 40;
int slow = 20;
-/*
-int fast = 80;
-int normal = 60;
-int slow = 40;
-*/
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
-//手動モード
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
@@ -229,8 +215,6 @@
while(1) {
//pc.printf("4:%d\n\r",g[4]);
- //LED_DEBUG0 = !LED_DEBUG0;
-
if(EMC==0) {
fast=40;
normal=30;
@@ -243,8 +227,6 @@
Air_flag = false;
zyouge=false;
zyougedo=false;
- dz1=true;
- dz1i=false;
dz2=true;
dz2i=false;
dz3=true;
@@ -412,10 +394,10 @@
motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
- motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
- motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
- motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
- motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
+ motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
if(controller->Button.UP||controller->Button.DOWN) {
@@ -1927,13 +1909,13 @@
fast=40;
normal=30;
slow=20;
- motor[TIRE_FR].pwm = normal+10;
+ motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal+10;
+ motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+10;
+ motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+10;
+ motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = FOR;
if(g[2]==0&&countss>=100000) {
countss=100000;
@@ -1943,9 +1925,9 @@
} else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 25;
+ motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -1956,87 +1938,94 @@
} else if(mode==10) { //縦ライントレース
switch(g[0]) {
case 98:
- motor[TIRE_FR].pwm = normal;
+ motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = normal;
+ motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal;
+ motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal;
+ motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = FOR;
mtc=true;
if(countss>80000) {
cross++;
countss=0;
}
+ /*
+ if(cross==2){
+ fast = 30;
+ normal = 20;
+ slow = 20;
+ }
+ */
if(cross==3) {
mode=11;
countss=0;
cross=0;
}
break;
- //+20した
+ //+15した
case 0:
- motor[TIRE_FR].pwm = fast+15;
+ motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast+15;
+ motor[TIRE_FL].pwm = fast+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+15;
+ motor[TIRE_BR].pwm = fast+20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+15;
+ motor[TIRE_BL].pwm = fast+20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
- motor[TIRE_FR].pwm = fast+15;
+ motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = slow+15;
+ motor[TIRE_FL].pwm = slow+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+15;
+ motor[TIRE_BR].pwm = slow+20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+15;
+ motor[TIRE_BL].pwm = fast+20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
- motor[TIRE_FR].pwm = slow+15;
+ motor[TIRE_FR].pwm = slow+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+15;
+ motor[TIRE_BL].pwm = slow+20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
- motor[TIRE_FR].pwm = normal+15;
+ motor[TIRE_FR].pwm = normal+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 = normal+15;
+ motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
- motor[TIRE_FR].pwm = slow+15;
+ motor[TIRE_FR].pwm = slow+20;
motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = fast+15;
+ motor[TIRE_FL].pwm = fast+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+15;
+ motor[TIRE_BR].pwm = fast+20;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = slow+15;
+ motor[TIRE_BL].pwm = slow+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+15;
+ motor[TIRE_FL].pwm = slow+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+15;
+ motor[TIRE_BR].pwm = slow+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -2045,9 +2034,9 @@
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal+15;
+ motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+15;
+ motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -2105,19 +2094,20 @@
solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
case 98:
- motor[TIRE_FR].pwm = normal+5;
+ motor[TIRE_FR].pwm = normal+10;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = normal+5;
+ motor[TIRE_FL].pwm = normal+10;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+5;
+ motor[TIRE_BR].pwm = normal+10;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+5;
+ motor[TIRE_BL].pwm = normal+10;
motor[TIRE_BL].dir = BACK;
if(countss>30000) {
cross++;
countss=0;
}
mtc2=true;
+ //change
if(cross==1) {
fast=30;
normal=20;
@@ -2138,33 +2128,33 @@
}
break;
case 0:
- motor[TIRE_FR].pwm = fast+5;
+ motor[TIRE_FR].pwm = fast+15;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast+5;
+ motor[TIRE_FL].pwm = fast+15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+5;
+ motor[TIRE_BR].pwm = fast+15;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+5;
+ motor[TIRE_BL].pwm = fast+15;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 255:
- motor[TIRE_FR].pwm = normal+5;
+ motor[TIRE_FR].pwm = normal+15;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = fast+5;
+ motor[TIRE_FL].pwm = fast+15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = fast+5;
+ motor[TIRE_BR].pwm = fast+15;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = normal+5;
+ motor[TIRE_BL].pwm = normal+15;
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+5;
+ motor[TIRE_FL].pwm = slow+15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+5;
+ motor[TIRE_BR].pwm = slow+15;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
@@ -2173,44 +2163,44 @@
case 254:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = normal+5;
+ motor[TIRE_FL].pwm = normal+15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = normal+5;
+ motor[TIRE_BR].pwm = normal+15;
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+5;
+ motor[TIRE_FR].pwm = fast+15;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = slow+5;
+ motor[TIRE_FL].pwm = slow+15;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].pwm = slow+5;
+ motor[TIRE_BR].pwm = slow+15;
motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].pwm = fast+5;
+ motor[TIRE_BL].pwm = fast+15;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 3:
- motor[TIRE_FR].pwm = slow+5;
+ motor[TIRE_FR].pwm = slow+15;
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+5;
+ motor[TIRE_BL].pwm = slow+15;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 2:
- motor[TIRE_FR].pwm = normal+5;
+ motor[TIRE_FR].pwm = normal+15;
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+5;
+ motor[TIRE_BL].pwm = normal+15;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
@@ -2476,9 +2466,10 @@
motor[MOTOR_0].dir = BRAKE;
}
if(g[1]==98) {
- fast=30;
- normal=20;
- slow=20;
+ //Change
+ fast=20;
+ normal=15;
+ slow=10;
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
@@ -2493,7 +2484,7 @@
countss=0;
fast=60;
normal=40;
- slow=30;
+ slow=20;
}
} else if(mode==24) {
if(DOLS==true||UPLS==true) {
@@ -3053,13 +3044,13 @@
} else if(mode==33) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
- if(g[3]==0) {
+ if(g[3]==0&&g[2]==0) {
cross=0;
countss=0;
mode=40;
@@ -3075,29 +3066,11 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
- if(countss>5000) {
+ if(countss>10000) {
cross++;
countss=0;
}
mtc2=true;
- if(LIF||LIB) {
- mode=41;
- countss=0;
- cross=0;
- }
- if(YELLOW) {
- if(cross==3) {
- mode=41;
- countss=0;
- cross=0;
- }
- } else {
- if(cross==3) {
- mode=41;
- countss=0;
- cross=0;
- }
- }
break;
case 0:
motor[TIRE_FR].pwm = fast;
@@ -3210,33 +3183,53 @@
break;
}
}
+
+ if(LIF||LIB) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ if(YELLOW) {
+ if(cross==1) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ } else {
+ if(cross==3) {
+ mode=41;
+ countss=0;
+ cross=0;
+ }
+ }
+
} 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].pwm = 40;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[1]==0) {
countss=0;
mode=70;
- normal=60;
- fast=70;
- slow=40;
+ normal=80;
+ fast=100;
+ slow=60;
}
} else if(mode==42) {
- motor[TIRE_FR].pwm = 20;
+ 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 = 20;
+ motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
countss=0;
@@ -3260,13 +3253,13 @@
motor[TIRE_BL].dir = BACK;
break;
case 0:
- motor[TIRE_FR].pwm = 80;
+ motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 80;
+ motor[TIRE_FL].pwm = 100;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 80;
+ motor[TIRE_BR].pwm = 100;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 80;
+ motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
@@ -3371,13 +3364,13 @@
}
}
} else if(mode==100) {
- motor[TIRE_FR].pwm = 40;
+ motor[TIRE_FR].pwm = 50;
motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 40;
+ motor[TIRE_FL].pwm = 50;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].pwm = 43;
+ motor[TIRE_BR].pwm = 50;
motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].pwm = 40;
+ motor[TIRE_BL].pwm = 50;
motor[TIRE_BL].dir = BACK;
} else {
}
@@ -3388,99 +3381,6 @@
#if USE_PROCESS_NUM>4
static void Process4()
{
-//中間トレーステスト
- switch(g[4]) {
- case 98:
- 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-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 99:
- 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;
- 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-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 255:
- 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;
- break;
- case 253:
- 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-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 254:
- motor[TIRE_FR].pwm = fast;
- 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 = fast-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 1:
- 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;
- break;
- case 3:
- 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-5;
- motor[TIRE_BL].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = normal;
- 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 = normal-5;
- motor[TIRE_BL].dir = FOR;
- break;
- }
}
#endif
@@ -3488,544 +3388,13 @@
#if USE_PROCESS_NUM>5
static void Process5()
{
- if(g[4]==3||g[4]==2) {
- 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[4]==253||g[4]==254) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- }
- switch(g[2]) {
- case 0:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 255:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- break;
- case 1:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- case 3:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- }
- switch(g[3]) {
- case 0:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 255:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- case 253:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 254:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FL].pwm = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 2:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- break;
- }
+
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
-//修正して中間トレーステスト
- if(testmode==0) {
- if(g[4]==3||g[4]==2) {
- 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[4]==253||g[4]==254) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- }
- switch(g[2]) {
- case 0:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 255:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- break;
- case 1:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- case 3:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- }
- switch(g[3]) {
- case 0:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 255:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- case 253:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 254:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FL].pwm = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 2:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- break;
- }
- if(g[3]==0&&g[2]==0) {
- testmode=1;
- }
- } else if(testmode==1) {
- switch(g[4]) {
- case 98:
- 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;
- break;
- case 99:
- 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;
- 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;
- break;
- case 255:
- 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;
- break;
- case 253:
- 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;
- break;
- case 254:
- motor[TIRE_FR].pwm = 40;
- 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 = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- 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;
- break;
- case 3:
- 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;
- break;
- case 2:
- motor[TIRE_FR].pwm = 60;
- 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 = 60;
- motor[TIRE_BL].dir = BACK;
- break;
- }
- if(g[1]==98) {
- testmode=2;
- }
- } else if(testmode==2) {
- if(g[4]==3||g[4]==2) {
- 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[4]==253||g[4]==254) {
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- 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 = FOR;
- }
- switch(g[2]) {
- case 0:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 255:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 253:
- motor[TIRE_FR].pwm = 40;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BR].dir = FREE;
- break;
- case 254:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = BACK;
- break;
- case 1:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- case 3:
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_BR].pwm = 40;
- motor[TIRE_BR].dir = FOR;
- break;
- case 2:
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BR].dir = FOR;
- break;
- }
- switch(g[3]) {
- case 0:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 255:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- case 253:
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FL].dir = FREE;
- motor[TIRE_BL].pwm = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 254:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 3:
- motor[TIRE_FL].pwm = 40;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 20;
- motor[TIRE_BL].dir = FREE;
- break;
- case 2:
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BL].dir = FOR;
- break;
- }
- if(g[3]==0&&g[2]==0) {
- testmode=3;
- }
- } else if(testmode==3) {
- switch(g[4]) {
- case 98:
- 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;
- break;
- case 99:
- 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;
- 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;
- break;
- case 255:
- 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;
- break;
- case 253:
- 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;
- break;
- case 254:
- motor[TIRE_FR].pwm = 40;
- 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 = 40;
- motor[TIRE_BL].dir = BACK;
- break;
- case 1:
- 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;
- break;
- case 3:
- 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;
- break;
- case 2:
- motor[TIRE_FR].pwm = 60;
- 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 = 60;
- motor[TIRE_BL].dir = BACK;
- break;
- }
- if(g[1]==98) {
- testmode=4;
- }
- } else if(testmode==4) {
- 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;
- }
}
#endif