大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 33:dd598bb23ad1
- Parent:
- 32:985ec7913f2a
- Child:
- 34:2fce61e3ebc4
diff -r 985ec7913f2a -r dd598bb23ad1 System/Process/Process.cpp
--- a/System/Process/Process.cpp Tue Oct 22 02:50:54 2019 +0000
+++ b/System/Process/Process.cpp Mon Oct 28 21:11:17 2019 +0000
@@ -62,11 +62,87 @@
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);
+}
+
+int selectnum[16][4]= {
+//1,2,4,8
+ {0,0,0,0},
+ {1,0,0,0},
+ {0,1,0,0},
+ {1,1,0,0},
+ {0,0,1,0},
+ {1,0,1,0},
+ {0,1,1,0},
+ {1,1,1,0},
+ {0,0,0,1},
+ {1,0,0,1},
+ {0,1,0,1},
+ {1,1,0,1},
+ {0,0,1,1},
+ {1,0,1,1},
+ {0,1,1,1},
+ {1,1,1,1}
+};
+
+DigitalIn EMC(PC_13);
+
+void LedMode(int num)
+{
+/*
+ POTENTIOMETER::dio[0]= selectnum[num][0];;
+ POTENTIOMETER::dio[1]= selectnum[num][1];;
+ POTENTIOMETER::dio[2]= selectnum[num][2];;
+ POTENTIOMETER::dio[3]= selectnum[num][3];;
+ */
+
+}
+
+bool start_flag = true;
+bool UP_flag = false;
+bool SW_flag = false;
+bool Air_flag = false;
+bool zyouge;
+bool zyougedo;
+bool dz1=true;
+bool dz1i=false;
+bool dz2=true;
+bool dz2i=false;
+bool dz3=true;
+bool dz3i=false;
+bool dz4=true;
+bool dz4i=false;
+bool zone;
+bool through=false;
+bool counts=false;
+bool mtc=false;
+bool mtc2 = false;
+bool emcs=false;
+bool kiri;
+
+int g[8];
+int Twsh;
+int mode =0;
+int cross=0;//十字数える用
+int cros=0;
+int countss=100000;//wait代替え
+
+///*********PWM調整用*********///
+int fast =60;
+int normal = 40;
+int slow = 20;
+
+bool MU_flag = false;
+
#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);
@@ -176,32 +252,163 @@
void SystemProcess()
{
SystemProcessInitialize();
-
+
while(1) {
+ //lock = false;
+
+ for(int i = 0; i < 8; i++) {
+ g[i] = LineHub::GetPara(i);
+ }
+
+ if(EMC==0) {
+ fast=40;
+ normal=30;
+ slow=20;
+ cross=0;//十字数える用
+ cros=0;
+ countss=0;//wait代替え
+ UP_flag = false;
+ SW_flag = false;
+ Air_flag = false;
+ zyouge=false;
+ zyougedo=false;
+ dz1=true;
+ dz1i=false;
+ dz2=true;
+ dz2i=false;
+ dz3=true;
+ dz3i=false;
+ dz4=true;
+ dz4i=false;
+ through=false;
+ counts=false;
+ mtc=false;
+ mtc2 = false;
+ current = 0;
+ SystemProcessInitialize();
+ if(emcs==false) {
+ AllActuatorReset();
+ emcs=true;
+ }
+ } else {
+ emcs=false;
+ }
- #ifdef USE_MU
+ if(mode==0) {
+ if(ZoneSW) {
+ LedMode(1);
+ } else {
+ LedMode(2);
+ }
+ }
+ for(int i = 0; i < 8; i++) {
+ g[i] = LineHub::GetPara(i);
+ }
+ if(ThSW) {
+ Twsh=1;
+ } else {
+ Twsh=2;
+ }
+
+ if(StertSW && start_flag == true) {
+ start_flag = false;
+ lock = false;
+ if(ZoneSW) {
+ countss=0;
+ mode=1;
+ current = 6;
+
+ } else {
+ countss=0;
+ mode=1;
+ current = 4;
+ }
+ }
+ if(DWSW||DOSW) {
+ if(DWSW) {
+ zyouge=true;
+ }
+ if(DOSW) {
+ if(DOLS) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ } else {
+ motor[MOTOR_0].pwm = 50;
+ motor[MOTOR_0].dir = BACK;
+ }
+ }
+ } else if(zyouge==false) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ if(zyouge) {
+ if(Twsh==1) {
+ motor[MOTOR_0].pwm = 215;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ zyouge=false;
+ }
+ }
+ if(Twsh==2) {
+ motor[MOTOR_0].pwm = 245;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS2) {
+ motor[MOTOR_0].pwm = 100;
+ motor[MOTOR_0].dir = BRAKE;
+ zyouge=false;
+ }
+ }
+ }
+ if(AIRSW) {
+ if(SW_flag==false) {
+ if(Air_flag==false) {
+ solenoid.solenoid3 = SOLENOID_ON;
+ Air_flag=true;
+ SW_flag=true;
+ } else {
+ solenoid.solenoid3 = SOLENOID_OFF;
+ Air_flag=false;
+ SW_flag=true;
+ }
+ }
+ } else {
+ if(SW_flag==true) {
+ countss++;
+ if(countss>=2000) {
+ SW_flag=false;
+ countss=0;
+ }
+ }
+ }
+
+
+#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
- #endif
+#endif
+
- #ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
+#ifdef USE_ERRORCHECK
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
} else
- #endif
+
+#endif
{
- #ifdef USE_SUBPROCESS
+#ifdef USE_SUBPROCESS
if(!lock) {
Process[current]();
} else
- #endif
+#endif
{
//ロック時の処理
}
}
-
+
SystemProcessUpdate();
}
}
@@ -214,35 +421,1813 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
- AllActuatorReset();
+
+ //AllActuatorReset();
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
+ LedMode(6);
+ start_flag = true;
+ mtc=false;
+ mtc2 = false;
+ mode=1;
+ cross=0;
+ Twsh=1;
+ motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
+ 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;
+
+ if(controller->Button.UP||controller->Button.DOWN) {
+
+ if(controller->Button.UP) {
+ motor[MOTOR_0].pwm =200;
+ motor[MOTOR_0].dir = FOR;
+ if(UPLS) {
+ motor[MOTOR_0].pwm = 0;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+ }
+ if(controller->Button.DOWN) {
+ motor[MOTOR_0].pwm = 50;
+ motor[MOTOR_0].dir = BACK;
+ }
+ } else {
+ motor[MOTOR_0].pwm = 0;
+ motor[MOTOR_0].dir = BRAKE;
+ }
+
+ if(controller->Button.Y) {
+ if(dz2==true) {
+ if(dz2i==false) {
+ solenoid.solenoid2 = SOLENOID_ON;
+ dz2i=true;
+ } else {
+ solenoid.solenoid2 = SOLENOID_OFF;
+ dz2i=false;
+ }
+ dz2=false;
+ }
+ } else {
+ dz2=true;
+ }
+
+ if(controller->Button.A) {
+ if(dz3==true) {
+ if(dz3i==false) {
+
+ dz3i=true;
+ } else {
+ solenoid.solenoid3 = SOLENOID_OFF;
+ dz3i=false;
+ }
+ dz3=false;
+ }
+ } else {
+ dz3=true;
+ }
+
+ if(controller->Button.B) {
+ if(dz4==true) {
+ if(dz4i==false) {
+ solenoid.solenoid4 = SOLENOID_ON;
+ dz4i=true;
+ } else {
+ solenoid.solenoid4 = SOLENOID_OFF;
+ dz4i=false;
+ }
+ dz4=false;
+ }
+ } else {
+ dz4=true;
+ }
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
{
+ //Oh So 青ゾーン
+ countss++;
+ LedMode(4);
+ if(mode==1) {//スタートゾーンから白線検知
+ fast=40;
+ normal=30;
+ slow=20;
+ motor[TIRE_FR].pwm = 27;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].pwm = 33;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].pwm = 33;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].pwm = 27;
+ motor[TIRE_BL].dir = BACK;
+ //スタート後どれくらい無検知でうごくか↓
+ if(g[2]==0&&countss>=100000) {
+ countss=100000;
+ cross=0;
+ mode=3;
+ }
+ } else if(mode==3) { //横ライントレースから縦ライントレースへ
+ 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;
+ if(g[1]==0) {
+ mode=10;
+ countss=100000;
+ }
+ } else if(mode==10) { //縦ライントレース
+ if(countss==false&&g[2]==0) {
+ cros++;
+ countss=true;
+ }
+ if(countss==true&&g[2]==99) {
+ countss=false;
+ }
+
+ if(cros==Twsh) {
+ mode=11;
+ countss=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(countss>100000) {
+ cross++;
+ countss=0;
+ }
+ if(cross==Twsh) {
+ mode=11;
+ countss=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) {
+ countss=0;
+ cross=0;
+ mode=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;
+ //countss=0;
+ //cros=0;
+ }//ゆっくりモードに入れなかった時の保険
+ if(g[1]==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(countss>50000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=22;
+ cross=0;
+ countss=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 = BRAKE;
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BL].dir = BRAKE;
+ if(countss<=100000) {
+ mode=22;
+ }
+ }
+ 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(countss>=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;
+ countss=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) {
+ countss=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(countss>=10000) {
+ mode=26;
+ countss=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(countss>=5000) {
+ mode=27;
+ countss=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(countss>=4000) {
+ countss=0;
+ mode=28;
+ }
+ } else if(mode==28) {
+ 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(countss>=10000) {
+ mode=30;
+ countss=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&&countss>=10000) {
+ mode=30;
+ countss=0;
+ cros=0;
+ }
+ }
+ }
+ 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 = 57;
+ motor[TIRE_BL].dir = BACK;
+ if(g[1]==98) {
+ mode=31;
+ countss=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(countss>=5000) {
+ mode=32;
+ countss=0;
+ }
+ } 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(countss>=30000) {
+ mode=33;
+ countss=0;
+ }
+ } else if(mode==33) {
+ 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;
+ if(g[3]==0&&countss>=20000) {
+ countss=0;
+ if(Twsh==2) {
+ solenoid.solenoid2 = SOLENOID_OFF;
+ }
+ mode=40;
+ }
+ } else if(mode==40) {
+ if(LIB) {
+ mode=42;
+ }
+ 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(countss>=10000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=41;
+ countss=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) {
+ if(LIF||LIB) {
+ mode=42;
+ }
+ 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) {
+ countss=0;
+ mode=70;
+ normal=60;
+ fast=70;
+ slow=40;
+ }
+ } else if(mode==42) {
+ motor[TIRE_FR].pwm = 0;
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].pwm = 0;
+ motor[TIRE_BL].dir = FREE;
+ if(g[0]==0) {
+ countss=0;
+ slow=40;
+ }
+
+ } else if(mode==70)
+ switch(g[0]) {
+ case 99:
+ if(countss>=100000) {
+ cross++;
+ mode=100;
+ countss=0;
+ }
+ if(cross==2) {
+ 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;
+ }
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
-
+//Clearly 赤ゾーン
+ LedMode(3);
+ countss++;
+ 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&&countss>=100000) {
+ countss=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;
+ countss=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;
+ countss=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(countss>80000) {
+ cross++;
+ countss=0;
+ }
+ if(cross==Twsh) {
+ mode=11;
+ countss=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) {
+ countss=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;
+ // countss=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(countss>30000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=22;
+ countss=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&&countss>=10000) {
+ mode=22;
+ countss=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(countss>=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;
+ countss=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) {
+ countss=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(countss>=10000) {
+ mode=26;
+ countss=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(countss>=5000) {
+ mode=27;
+ countss=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(countss>=100) {
+ countss=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(countss>=10000) {
+ mode=30;
+ countss=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&&countss>=10000) {
+ mode=30;
+ countss=0;
+ 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;
+ countss=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(countss>=5000) {
+ countss=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(countss>=40000) {
+ mode=33;
+ countss=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&&countss>=20000) {
+ countss=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;
+ //countss=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(countss>20000) {
+ cross++;
+ countss=0;
+ }
+ mtc2=true;
+ if(cross==2) {
+ mode=41;
+ countss=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) {
+ countss=0;
+ mode=70;
+ normal=60;
+ fast=70;
+ slow=40;
+ }
+ } 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) {
+ countss=0;
+ mode=70;
+ }
+ } else if(mode==70) {
+ switch(g[1]) {
+ case 99:
+ if(countss>=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
#if USE_PROCESS_NUM>4
static void Process4()
{
-
+
}
#endif
@@ -263,14 +2248,14 @@
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+
}
#endif
#if USE_PROCESS_NUM>8
static void Process8()
{
-
+
}
#endif
@@ -285,16 +2270,16 @@
static void AllActuatorReset()
{
- #ifdef USE_SOLENOID
- solenoid.all = ALL_SOLENOID_OFF;
- #endif
-
- #ifdef USE_MOTOR
- for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
- motor[i].dir = FREE;
- motor[i].pwm = 0;
- }
- #endif
+#ifdef USE_SOLENOID
+ solenoid.all = ALL_SOLENOID_OFF;
+#endif
+
+#ifdef USE_MOTOR
+ for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+ motor[i].dir = FREE;
+ motor[i].pwm = 0;
+ }
+#endif
}
#pragma region USER-DEFINED-FUNCTIONS