大季 矢花
/
MB2019_main_alltimes_old
aa
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2019-10-29
- Revision:
- 34:2fce61e3ebc4
- Parent:
- 33:dd598bb23ad1
- Child:
- 35:33c6cdba5630
File content as of revision 34:2fce61e3ebc4:
#include "mbed.h" #include "Process.h" #include <stdlib.h> #include "../../CommonLibraries/PID/PID.h" #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" #include "../../Communication/RS485/LineHub/LineHub.h" #include "../../Communication/Controller/Controller.h" #include "../../Switch/Switch.h" #include "../../LED/LED.h" #include "../../Safty/Safty.h" #include "../Using.h" using namespace SWITCH; using namespace PID_SPACE; using namespace LINEHUB; static CONTROLLER::ControllerData *controller; ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; static bool lock; static bool processChangeComp; static int current; static void AllActuatorReset(); #ifdef USE_SUBPROCESS static void (*Process[USE_PROCESS_NUM])(void); #endif #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE /*Replace here with the definition code of your variables.*/ //Serial pc(USBTX, USBRX); const int omni[15][15] = { { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 }, { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 }, { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 }, { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 }, { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 }, { -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 }, { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 }, { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 }, { -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 }, { -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 }, { -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 }, { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 }, { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 }, { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 }, { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 } }; 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); } 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; #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); #endif #if USE_PROCESS_NUM>2 static void Process2(void); #endif #if USE_PROCESS_NUM>3 static void Process3(void); #endif #if USE_PROCESS_NUM>4 static void Process4(void); #endif #if USE_PROCESS_NUM>5 static void Process5(void); #endif #if USE_PROCESS_NUM>6 static void Process6(void); #endif #if USE_PROCESS_NUM>7 static void Process7(void); #endif #if USE_PROCESS_NUM>8 static void Process8(void); #endif #if USE_PROCESS_NUM>9 static void Process9(void); #endif #endif void SystemProcessInitialize() { #pragma region USER-DEFINED_VARIABLE_INIT /*Replace here with the initialization code of your variables.*/ #pragma endregion USER-DEFINED_VARIABLE_INIT lock = true; processChangeComp = true; current = DEFAULT_PROCESS; #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 Process[0] = Process0; #endif #if USE_PROCESS_NUM>1 Process[1] = Process1; #endif #if USE_PROCESS_NUM>2 Process[2] = Process2; #endif #if USE_PROCESS_NUM>3 Process[3] = Process3; #endif #if USE_PROCESS_NUM>4 Process[4] = Process4; #endif #if USE_PROCESS_NUM>5 Process[5] = Process5; #endif #if USE_PROCESS_NUM>6 Process[6] = Process6; #endif #if USE_PROCESS_NUM>7 Process[7] = Process7; #endif #if USE_PROCESS_NUM>8 Process[8] = Process8; #endif #if USE_PROCESS_NUM>9 Process[9] = Process9; #endif #endif } static void SystemProcessUpdate() { #ifdef USE_SUBPROCESS if(controller->Button.HOME) lock = false; if(controller->Button.START && processChangeComp) { current++; if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; processChangeComp = false; } else if(controller->Button.SELECT && processChangeComp) { current--; if (current < 0) current = 0; processChangeComp = false; } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; #endif #ifdef USE_MOTOR ACTUATORHUB::MOTOR::Motor::Update(motor); #endif #ifdef USE_SOLENOID ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); #endif #ifdef USE_RS485 ACTUATORHUB::ActuatorHub::Update(); //LINEHUB::LineHub::Update(); #endif } void SystemProcess() { SystemProcessInitialize(); while(1) { /* if(StertSW){ LED_DEBUG0=1; } else{ LED_DEBUG0=0; } */ 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; } 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 = 3; } else { countss = 0; mode=1; current = 2; } } 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 #ifdef USE_ERRORCHECK if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag){ CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; } else #endif { #ifdef USE_SUBPROCESS if(!lock) { Process[current](); } else #endif { //ロック時の処理 } } SystemProcessUpdate(); } } #pragma region PROCESS #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0() { //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 #if USE_PROCESS_NUM>5 static void Process5() { } #endif #if USE_PROCESS_NUM>6 static void Process6() { } #endif #if USE_PROCESS_NUM>7 static void Process7() { } #endif #if USE_PROCESS_NUM>8 static void Process8() { } #endif #if USE_PROCESS_NUM>9 static void Process9() { } #endif #endif #pragma endregion PROCESS 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 } #pragma region USER-DEFINED-FUNCTIONS uint8_t SetStatus(int pwmVal) { if (pwmVal < 0) return BACK; else if (pwmVal > 0) return FOR; else if (pwmVal == 0) return BRAKE; else return BRAKE; } uint8_t SetPWM(int pwmVal) { if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; else return abs(pwmVal); } #pragma endregion