![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp
- Committer:
- M_souta
- Date:
- 2019-09-28
- Revision:
- 26:4c0ce2f05688
- Parent:
- 25:b3a9f34b201d
File content as of revision 26:4c0ce2f05688:
#include "mbed.h" #include "Process.h" #include "../../CommonLibraries/PID/PID.h" #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" #include "../../Communication/RS485/LineHub/LineHub.h" #include "../../Communication/Controller/Controller.h" #include "../../Input/ExternalInt/ExternalInt.h" #include "../../Input/Switch/Switch.h" #include "../../Input/Encoder/Encoder.h" #include "../../Input/Ultrasonic/Ultrasonic.h" #include "../../LED/LED.h" #include "../../Safty/Safty.h" #include "../Using.h" using namespace SWITCH; using namespace PID_SPACE; using namespace ENCODER; using namespace ULTRASONIC; 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.*/ USS ultrasonic[] = { USS(ECHO_0,TRIG_0,TEMP), USS(ECHO_1,TRIG_1,TEMP), }; Serial pc(USBTX, USBRX); //**************Buzzer**************** //DigitalOut buzzer(BUZZER_PIN); void BuzzerTimer_func(); Ticker BuzzerTimer; bool EMGflag = false; PwmOut buzzer(BUZZER_PIN); //**************Buzzer**************** //************TapeLed***************** void TapeLedEms_func(); TapeLedData tapeLED; TapeLedData sendLedData; TapeLED_Mode ledMode = Normal; Ticker tapeLedTimer; //************TapaLed***************** //*************** lift *************** #define LOWER 1 #define MIDDLRE 2 #define UPPER 3 uint8_t liftState = LOWER; bool moving = false; bool switchFlag_LB = false; bool switchFlag_RB = false; //*************** lift *************** //*************tire************* PID rotaconPID[] = { PID(0.0001,-1,1,0.05,0,0), //LF PID(0.0001,-1,1,0.05,0,0), //LB PID(0.0001,-1,1,0.05,0,0), //RB PID(0.0001,-1,1,0.05,0,0), //RF }; #define FL 0 #define BL 1 #define BR 2 #define FR 3 #define PI 3.141592 const float tireR = 101.6; //タイヤの半径 const float ucR = 420.0; //中心からのタイヤの距離 typedef struct { float Vx; //X方向の速度 float Vy; //Y方向の速度 float Va; //角速度 } Vvector; Vvector move; //進む速度 Vvector correction_LT; //ライントレースの補正速度 Vvector synthetic; //合成速度 float sita = 0; bool PIDflag = false; int linePara[8]; int linePara_U; int linePara_B; int linePara_L; int linePara_R; #define FL 0 #define BL 1 #define BR 2 #define FR 3 float tireProcessRPM[4]; float tireTargetMaxRPM[4]; float tireTargetRPM[4]; float tirePWM[4]; float timePV[4]; float timeCV[4]; float pulsePV[4]; float pulseCV[4]; void tirePID(); int lineCast(char k); Timer rotaconSampling; Ticker rotaconPIDtimer; bool countFlag; //*************tire**************// // ************* Line ************** // Timer tow_stop; float pw = 0; int lineFase = 0; bool lineCheck = false; bool lineFlag = false; bool adjAnable = false; int adj = 0; int countW = 0; int lineCount = 0; int targetCount = 0; bool startFlag = true; int linePWM; int adj_F; int adj_B; int mode = 0; // ************* Line ************** // 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 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); uint8_t SetPWM(int pwmVal) { if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; else return abs(pwmVal); } #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.*/ //rotaconPIDtimer.attach(tirePID,0.1); //DigitalOut Air_16(LS_16); //DigitalOut Air_17(LS_17); //DigitalOut Air_18(LS_18); //DigitalOut Air_19(LS_19); #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) { int g[8]; for(int i = 0; i < 8; i++){ g[i] = lineCast(LineHub::GetPara(i)); } pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]); if(LimitSw::IsPressed(START_SW) && startFlag == true) { startFlag == false; lock = false; lineFase = 0; lineCount = 0; lineCheck = false; countW = 0; if(LimitSw::IsPressed(REDBLUE_SW)) { current = 4; } else { current = 5; } } buzzer.period(1.0/800); #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif #ifdef USE_ERRORCHECK if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag) { CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; } else #endif { #ifdef USE_SUBPROCESS if(!lock) { Process[current](); } else #endif { //ロック時の処理 } } //Emergency! /* if(!EMG_0 && !EMG_1 && !EMGflag){ buzzer = 0; BuzzerTimer.attach(BuzzerTimer_func, 1); EMGflag = true; LED_DEBUG0 = 1; } if(EMG_0 && EMG_1 && EMGflag){ buzzer = 1; BuzzerTimer.detach(); EMGflag = false; } */ 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() { PIDflag = false; if(controller->Button.UP) { motor[LIFT_LB].dir = FOR; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = BACK; motor[LIFT_RB].pwm = 200; } else if(controller->Button.DOWN) { motor[LIFT_LB].dir = BACK; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = FOR; motor[LIFT_RB].pwm = 200; } else if(controller->Button.LEFT) { motor[LIFT_LB].dir = FOR; motor[LIFT_LB].pwm = 180; } else if(controller->Button.RIGHT) { motor[LIFT_RB].dir = BACK; motor[LIFT_RB].pwm = 180; } else { motor[LIFT_LB].dir = BRAKE; motor[LIFT_LB].pwm = 255; motor[LIFT_RB].dir = BRAKE; motor[LIFT_RB].pwm = 255; } if(controller->Button.X) { motor[LIFT_U].dir = FOR; motor[LIFT_U].pwm = 180; } else if(controller->Button.Y) { motor[LIFT_U].dir = BACK; motor[LIFT_U].pwm = 230; } else { motor[LIFT_U].dir = BRAKE; motor[LIFT_U].pwm = 255; } if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) { motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] ); motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] ); motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] ); motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] ); motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ; motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ; motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ; } else { motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]); motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]); motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]); motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]); motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]); motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]); motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]); motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]); } if(controller->Button.ZR) { solenoid.solenoid4 = SOLENOID_ON; } if(controller->Button.ZL) { solenoid.solenoid4 = SOLENOID_OFF; } } #endif #if USE_PROCESS_NUM>2 static void Process2() { /* if(moving) { if(LimitSw::IsPressed(LSW_LB)) { if(switchFlag_LB) { switchFlag_LB = false; motor[LIFT_LB].dir = BRAKE; motor[LIFT_LB].pwm = 200; } else { switchFlag_LB = true; } } if(LimitSw::IsPressed(LSW_RB)) { if(switchFlag_RB) { switchFlag_RB = false; motor[LIFT_RB].dir = BRAKE; motor[LIFT_RB].pwm = 200; } else { switchFlag_RB = true; } } if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false; } else { if(controller->Button.UP) { if(!liftSstate == UPPER)) { liftState++; motor[LIFT_LB].dir = FOR; motor[LIFT_RB].dir = BACK; motor[LIFT_LB].pwm = 185; motor[LIFT_RB].pwm = 180; } } else if(controller->Button.DOWN) { if(!(liftstate == LOWER)) { liftState--; moving = true; motor[LIFT_LB].dir = BACK; motor[LIFT_RB].dir = FOR; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].pwm = 185; } } else { motor[LIFT_LB].dir = BRAKE; motor[LIFT_RB].dir = BRAKE; motor[LIFT_LB].pwm = 200; motor[LIFT_RB].pwm = 200; } } */ } #endif #if USE_PROCESS_NUM>3 static void Process3() { startFlag = true; AllActuatorReset(); lineFase = 0; lineCheck = false; lineCount = 0; countW = 0; } #endif #if USE_PROCESS_NUM>4 static void Process4() { LED_DEBUG0 = LED_ON; /* ************************************** // 赤ゾーン 赤ゾーン 赤ゾーン // ************************************** */ for(int i = 0; i < 8; i++) { linePara[i] = lineCast(LineHub::GetPara(i)); } if(lineFase == 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[0] != 'N' && linePara[0] != 'A') { lineFase = 1; } motor[TIRE_FL].pwm = 30; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].pwm = 30; } else if(lineFase == 1) { // 前 ライントレース switch(linePara[0]) { case -2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = tirePWM[TIRE_FL]; tirePWM[TIRE_BL] = tirePWM[TIRE_BL]; tirePWM[TIRE_BR] = tirePWM[TIRE_BR]; tirePWM[TIRE_FR] = tirePWM[TIRE_FR]; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 3) { countW = 0; lineFase = 2; lineCount = 0; lineCheck = false; } } else if(lineFase == 2) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[4] == 0) { lineFase = 3; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 3) { // 右 ライントレース switch(linePara[4]) { case -2: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) { targetCount = 3; } else if(LimitSw::IsPressed(TOWEL1_SW)) { targetCount = 3; } else { targetCount = 2; } if(countW == targetCount) { countW = 0; lineFase = 4; lineCount = 0; lineCheck = false; } } else if(lineFase == 4) { // 右 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = BACK; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = FOR; if (linePara[LINE_TOW_1] == 0) { lineFase = 6; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 14; motor[TIRE_BL].pwm = 14; motor[TIRE_BR].pwm = 14; motor[TIRE_FR].pwm = 14; } else if (lineFase == 5) { lineFase = 6; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } else if(lineFase == 6) { // タオル1 検知 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) { lineFase = 7; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(LimitSw::IsPressed(TOW_1L)) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(LimitSw::IsPressed(TOW_1R)) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } } else if(lineFase == 7) { // ライン 修正 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(linePara[LINE_TOW_1] > 0) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = FOR; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_1] < 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = BACK; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_1] == 0) { lineFase = 8; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } } else if(lineFase == 8) { // タオル1 解放 //solenoid.TOWEL1 = SOLENOID_ON; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; lineFase = 9; } else if(lineFase == 9) { // 前 switch(linePara[LINE_TOW_1]) { case 2: tirePWM[TIRE_FL] = 5; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = 5; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 15; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -15; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 15; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -15; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -2: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 5; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -5; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ adj = 0; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 1) { countW = 0; lineFase = 10; lineCount = 0; lineCheck = false; } } else if(lineFase == 10) { motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[4] == 0) { lineFase = 11; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 11) { switch(linePara[4]) { case -2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ adj = 0;//linePara[3]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 2) { countW = 0; lineFase = 12; lineCount = 0; lineCheck = false; } } else if(lineFase == 12) { // 右 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = BACK; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = FOR; if(linePara[LINE_TOW_2] == 0) { lineFase = 13; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if (lineFase == 13) { lineFase = 14; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } else if(lineFase == 14) { // タオル2 竿検知 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) { lineFase = 15; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(LimitSw::IsPressed(TOW_2L)) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(LimitSw::IsPressed(TOW_2R)) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } } else if(lineFase == 15 ){ // ライン 修正 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(linePara[LINE_TOW_2] > 0) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = FOR; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_2] < 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = BACK; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_2] == 0) { lineFase = 16; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } } else if(lineFase == 16) { // タオル2 解放 //solenoid.TOWEL2 = SOLENOID_ON; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; lineFase = 17; } else if(lineFase == 17) { // 前 switch(linePara[LINE_TOW_2]) { case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -2: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ adj = 0; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 1) { countW = 0; lineFase = 18; lineCount = 0; lineCheck = false; } } else if(lineFase == 18) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[4] != 'N') { lineFase = 19; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 19) { // 左 switch(linePara[3]) { case -2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = 40; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } /* if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) { targetCount = 3; } else if(LimitSw::IsPressed(TOWEL1_SW)) { targetCount = 3; } else { targetCount = 2; } */ targetCount = 3; if(countW == targetCount) { countW = 0; lineFase = 20; lineCount = 0; lineCheck = false; } } else if(lineFase == 20) { // 左 低速 motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].dir = BACK; if (linePara[2] == 'N') { //(LimitSw::IsPressed(SHEETS_SW)) { //ineFase = 20; //else if(LimitSw::IsPressed(TOWEL2_SW) { //ineFase = 14; //else { lineFase = 21; // motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 21) { switch(linePara[2]) { case -2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 3) { countW = 0; lineFase = 22; lineCount = 0; lineCheck = false; } } else if(lineFase == 22) { motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].dir = BACK; if (linePara[2] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 30; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].pwm = 30; } else{ motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } } #endif #if USE_PROCESS_NUM>5 static void Process5() { /* ************************************** // 青ゾーン 青ゾーン 青ゾーン // ************************************** */ for(int i = 0; i < 8; i++) { linePara[i] = lineCast(LineHub::GetPara(i)); } if(lineFase == 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[0] != 'N' && linePara[0] != 'A') { lineFase = 1; } motor[TIRE_FL].pwm = 30; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].pwm = 30; }else if(lineFase == 1) { // 前 ライントレース switch(linePara[0]) { case -2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 3) { countW = 0; lineFase = 2; lineCount = 0; lineCheck = false; } } else if(lineFase == 2) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[3] == 0) { lineFase = 3; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 3) { // 左 ライントレース switch(linePara[3]) { case -2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 1) { countW = 0; lineFase = 4; lineCount = 0; lineCheck = false; } } else if(lineFase == 4) { // 右 低速 motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].dir = BACK; if(linePara[LINE_TOW_1] == 0) { lineFase = 5; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if (lineFase == 5) { lineFase = 6; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } else if(lineFase == 6) { // タオル1 竿検知 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) { lineFase = 7; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(LimitSw::IsPressed(TOW_1L)) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 20; } else if(LimitSw::IsPressed(TOW_1R)) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 20; } else { switch(linePara[LINE_TOW_1]) { case -2: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -14; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 14; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -17; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 17; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -17; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 17; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -14; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 14; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case 'A': tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); } } else if(lineFase == 7) { // ライン 修正 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(linePara[LINE_TOW_1] > 0) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = FOR; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_1] < 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = BACK; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_1] == 0) { //tow_stop.reset(); //tow_stop.start(); lineFase = 8; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } } else if(lineFase == 8) { // タオル1 解放 //solenoid.TOWEL1 = SOLENOID_ON; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; lineFase = 9; } else if(lineFase == 9) { // 前 switch(linePara[LINE_TOW_1]) { case 2: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 14; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -14; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 17; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -17; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 17; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -17; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 14; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -14; adjAnable = true; break; case -2: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ adj = 0; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 1) { countW = 0; lineFase = 10; lineCount = 0; lineCheck = false; } } else if(lineFase == 10) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[3] == 0) { lineFase = 11; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 11) { // 左 switch(linePara[3]) { case -2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 2) { countW = 0; lineFase = 12; lineCount = 0; lineCheck = false; } } else if(lineFase == 12) { // 左 低速 motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].dir = BACK; if(linePara[LINE_TOW_2] == 0) { lineFase = 13; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if (lineFase == 13) { lineFase = 14; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } else if(lineFase == 14) { // タオル2 竿検知 if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) { lineFase = 15; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(LimitSw::IsPressed(TOW_2L)) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 20; } else if(LimitSw::IsPressed(TOW_2R)) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 20; } else { switch(linePara[LINE_TOW_2]) { case -2: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = 20; break; case -3: tirePWM[TIRE_FL] = -14; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 14; tirePWM[TIRE_FR] = 20; break; case -1: tirePWM[TIRE_FL] = -17; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 17; tirePWM[TIRE_FR] = 20; break; case 0: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 20; break; case 1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -17; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 17; break; case 3: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -14; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 14; break; case 2: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 10; break; case 'A': tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 20; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); } } else if(lineFase == 15 ){ // ライン 修正 if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else if(linePara[LINE_TOW_2] > 0) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = FOR; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_2] < 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 16; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].dir = BACK; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_2] == 0) { lineFase = 16; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } else { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 50; } } else if(lineFase == 16) { // タオル2 解放 //solenoid.TOWEL2 = SOLENOID_ON; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; lineFase = 17; } else if(lineFase == 17) { // 前 switch(linePara[LINE_TOW_2]) { case 2: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 14; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -14; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 17; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -17; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 17; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -17; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 14; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -14; adjAnable = true; break; case -2: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 10; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ adj = 0; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 1) { countW = 0; lineFase = 18; lineCount = 0; lineCheck = false; } } else if(lineFase == 18) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; if(linePara[4] == 0) { lineFase = 19; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 19) { // 右 switch(linePara[4]) { case -2: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 10; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -10; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } /* if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) { targetCount = 3; } else if(LimitSw::IsPressed(TOWEL1_SW)) { targetCount = 3; } else { targetCount = 2; } */ targetCount = 3; if(countW == targetCount) { countW = 0; lineFase = 20; lineCount = 0; lineCheck = false; } } else if(lineFase == 20) { // 右 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = BACK; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = FOR; if (linePara[2] == 0) { //(LimitSw::IsPressed(SHEETS_SW)) { //ineFase = 20; //else if(LimitSw::IsPressed(TOWEL2_SW) { //ineFase = 14; //else { lineFase = 21; // motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 16; motor[TIRE_BL].pwm = 16; motor[TIRE_BR].pwm = 16; motor[TIRE_FR].pwm = 16; } else if(lineFase == 21) { switch(linePara[2]) { case -2: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case -3: tirePWM[TIRE_FL] = -10; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 10; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = -20; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 20; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 0: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -20; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -10; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 10; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -30; tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; default: tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; } if(adjAnable){ if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0]; } else { adj = 0; } motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } if(countW == 2) { countW = 0; lineFase = 22; lineCount = 0; lineCheck = false; } } else if(lineFase == 22) { motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].dir = FOR; if (linePara[0] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } motor[TIRE_FL].pwm = 30; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].pwm = 30; } else{ motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } } #endif #if USE_PROCESS_NUM>6 static void Process6() { for(int i = 0; i < 8; i++) { linePara[i] = lineCast(LineHub::GetPara(i)); } if(lineFase == 0) { if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) { lineFase = 1; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 30; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 30; } else if(LimitSw::IsPressed(TOW_1L)) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 12; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 12; } else if(LimitSw::IsPressed(TOW_1R)) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 12; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 12; } else { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 18; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 18; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 18; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 18; } } else if(lineFase == 1) { if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 30; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 30; } else if(linePara[LINE_TOW_1] > 0) { motor[TIRE_FL].dir = BACK; motor[TIRE_FL].pwm = 15; motor[TIRE_BL].dir = FOR; motor[TIRE_BL].pwm = 15; motor[TIRE_BR].dir = FOR; motor[TIRE_BR].pwm = 15; motor[TIRE_FR].dir = BACK; motor[TIRE_FR].pwm = 15; } else if(linePara[LINE_TOW_1] < 0) { motor[TIRE_FL].dir = FOR; motor[TIRE_FL].pwm = 15; motor[TIRE_BL].dir = BACK; motor[TIRE_BL].pwm = 15; motor[TIRE_BR].dir = BACK; motor[TIRE_BR].pwm = 15; motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 15; } else if(linePara[LINE_TOW_1] == 0) { lineFase = 2; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 30; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 30; } else { motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 30; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 30; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 30; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FR].pwm = 30; } } else if(lineFase == 2) { //solenoid::solenoid1 = SOLENOID_ON } } #endif #if USE_PROCESS_NUM>7 static void Process7() { } #endif #if USE_PROCESS_NUM>8 static void Process8() { if(controller->Button.A) { rotaconSampling.start(); PIDflag = true; //linePara_U = LineHub::GetPara(0); //linePara_B = LineHub::GetPara(3); pulsePV[FL] = encoder[FL].getPulses(); pulsePV[BL] = encoder[BL].getPulses(); pulsePV[BR] = encoder[BR].getPulses(); pulsePV[FR] = encoder[FR].getPulses(); for (int i = 0; i < 4; i++) { timeCV[i] = timePV[i]; timePV[i] = rotaconSampling.read(); tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60; pulseCV[i] = pulsePV[i]; } move.Vx = 0.5; move.Vy = 0.5; move.Va = 0; correction_LT.Vx = 0; //0.1 * linePara_U; correction_LT.Vy = 0; correction_LT.Va = 0; synthetic.Vx = move.Vx + correction_LT.Vx; synthetic.Vy = move.Vy + correction_LT.Vy; synthetic.Va = move.Va + correction_LT.Va; sita = 0; //タイヤの目標速度算出 float sinR = 0.7071 * (float)sin(sita); float cosR = 0.7071 * (float)cos(sita); float nv = (60 * 1000) / ( 2.00 * PI * tireR); tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv; tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv; //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]); //PIDによるPWM算出 //モータの駆動 for (int i = 0; i < 4; i++) { if (tirePWM[i] > 255){ tirePWM[i] = 255; } else if (tirePWM[i] < -255) { tirePWM[i] = -255; } } for(int i = 0;i < 4;i++){ motor[i].dir = SetStatus(tirePWM[i]); motor[i].pwm = SetPWM(tirePWM[i]); } } else { PIDflag = false; rotaconSampling.stop(); rotaconSampling.reset(); for(int i = 0;i < 4;i++){ encoder[i].reset(); pulsePV[i] = 0; pulseCV[i] = 0; timePV[i] = 0; timeCV[i] = 0; tirePWM[i] = 0; motor[i].dir = SetStatus(tirePWM[i]); motor[i].pwm = SetPWM(tirePWM[i]); } } } #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 } void BuzzerTimer_func(){ buzzer = !buzzer; //LED_DEBUG0 = !LED_DEBUG0; } void TapeLedEms_func() { sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; } #pragma region USER-DEFINED-FUNCTIONS void tirePID() { if(PIDflag == true) { //加算するPID値の算出 rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]); rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]); rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]); rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]); //PID値の加算 tirePWM[FL] += rotaconPID[0].GetMV(); tirePWM[BL] += rotaconPID[1].GetMV(); tirePWM[FR] += rotaconPID[2].GetMV(); tirePWM[BR] += rotaconPID[3].GetMV(); } } int lineCast(char k) { int l; switch(k) { case 255: l = -1; break; case 254: l = -2; break; case 253: l = -3; break; default: l = k; } return l; } #pragma endregion