![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp
- Committer:
- M_souta
- Date:
- 2019-09-09
- Revision:
- 21:e3b58d675c1c
- Parent:
- 20:eae8c84f318c
- Child:
- 22:7d93f79a3686
File content as of revision 21:e3b58d675c1c:
#include "mbed.h" #include "Process.h" #include "QEI.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/Potentiometer/Potentiometer.h" #include "../../Input/Encoder/Encoder.h" #include "../../LED/LED.h" #include "../../Safty/Safty.h" #include "../Using.h" using namespace SWITCH; using namespace PID_SPACE; using namespace ENCODER; 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); //**************Encoder*************** const int PerRev = 256; QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING); QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING); QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING); QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING); //**************Encoder*************** //**************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***************** 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.*/ #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] = LineHub::GetPara(i); } printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]); buzzer.period(1.0/800); #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif #ifdef USE_ERRORCHECK if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { 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() { /* if(controller->Button.UP) { motor[LIFT_LB].dir = FOR; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = BACK; motor[LIFT_RB].pwm = 180; } else if(controller->Button.DOWN) { motor[LIFT_LB].dir = BACK; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = FOR; motor[LIFT_RB].pwm = 180; } else { motor[LIFT_LB].dir = BRAKE; motor[LIFT_LB].pwm = 200; motor[LIFT_RB].dir = BRAKE; motor[LIFT_RB].pwm = 200; } */ 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 = 180; } else { motor[LIFT_U].dir = BRAKE; motor[LIFT_U].pwm = 180; } if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) { motor[TIRE_BL].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_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] ); motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ; motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]) ; motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ; motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]) ; } else { motor[TIRE_BL].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_FR].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]); } } #endif #if USE_PROCESS_NUM>2 static void Process2() { } #endif #if USE_PROCESS_NUM>3 static void Process3() { } #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 } 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 #pragma endregion