daad
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 21:1f1e9c585da8
- Parent:
- 19:96a462583af9
- Child:
- 22:c47f29caded6
diff -r 05080fd09347 -r 1f1e9c585da8 System/Process/Process.cpp --- a/System/Process/Process.cpp Sun Sep 01 01:33:36 2019 +0000 +++ b/System/Process/Process.cpp Fri Sep 13 01:36:42 2019 +0000 @@ -5,6 +5,7 @@ #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" @@ -17,6 +18,7 @@ using namespace SWITCH; using namespace PID_SPACE; using namespace ENCODER; +using namespace LINEHUB; static CONTROLLER::ControllerData *controller; ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; @@ -51,7 +53,7 @@ void BuzzerTimer_func(); Ticker BuzzerTimer; bool EMGflag = false; -PwmOut buzzer(BUZZER_PIN); +//PWMOut buzzer(BUZZER_PIN); //**************Buzzer**************** //************TapeLed***************** @@ -64,7 +66,43 @@ float tireProRPM[4]; float tireTarRPM[4]; -float tirePWM[4]; +float tirepwm[4]; + +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] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 }; +//{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200} + +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 @@ -103,213 +141,495 @@ void SystemProcessInitialize() { - #pragma region USER-DEFINED_VARIABLE_INIT - /*Replace here with the initialization code of your variables.*/ - #pragma endregion USER-DEFINED_VARIABLE_INIT + #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; + 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 +#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_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(); +#endif + +} + +int g[8]; + +void SystemProcess() +{ + SystemProcessInitialize(); + + while(1) { + for(int i = 0; i < 8; i++) { + g[i] = LineHub::GetPara(i); + } - #ifdef USE_RS485 - ACTUATORHUB::ActuatorHub::Update(); - #endif - +#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(); + } } -void SystemProcess() -{ - SystemProcessInitialize(); - - while(1) - { - if(LimitSw::IsPressed(10)) { - LED_DEBUG0 = LED_ON; - } else { - LED_DEBUG0 = LED_OFF; - } - //printf("%d\r\n",ECD_0.getPulses()); - 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() -{ - +static void Process0() +{ + } #endif #if USE_PROCESS_NUM>1 + + +bool dz1=true; +bool dz1i=false; +bool dz2=true; +bool dz2i=false; +bool dz3=true; +bool dz3i=false; +bool dz4=true; +bool dz4i=false; + +int mode=0; +int mode1=0; +int mode1g0=100000; + static void Process1() { - + mode1g0=100000; + mode=0; + mode1=0; + 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 = 110; + motor[MOTOR_0].dir = FOR; + } + 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.X) { + if(dz1==true) { + if(dz1i==false) { + solenoid.solenoid1 = SOLENOID_ON; + dz1i=true; + } else { + solenoid.solenoid1 = SOLENOID_OFF; + dz1i=false; + } + dz1=false; + } + } else { + dz1=true; + } + + 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) { + solenoid.solenoid3 = SOLENOID_ON; + 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; + } + /* + if(controller->Button.RIGHT){ + motor[MOTOR_1].dir = FOR; + motor[MOTOR_1].pwm = 60; + if (ARM_1){ + motor[MOTOR_1].dir = BRAKE; + } + } + else if(controller->Button.LEFT){ + motor[MOTOR_1].dir = BACK; + motor[MOTOR_1].pwm = 60; + if (ARM_0){ + motor[MOTOR_1].dir = BRAKE; + } + } + */ } #endif #if USE_PROCESS_NUM>2 +bool mtc=false; + static void Process2() -{ - +{ +// 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]); + mode1g0++; + if(g[0]!=99&&mode==0) { + switch(g[0]) { + case 98: + motor[TIRE_FR].pwm = 30; + 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 = 30; + motor[TIRE_BL].dir = FOR; + if(mode1g0>100000) { + mode1++; + mode1g0=0; + } + mtc=true; + if(mode1==2) { + mode=1; + } + break; + case 0: + motor[TIRE_FR].pwm = 60; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 60; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 60; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 255: + motor[TIRE_FR].pwm = 60; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 40; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 60; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 253: + motor[TIRE_FR].pwm = 20; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + motor[TIRE_BL].pwm = 20; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 254: + 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; + mtc=true; + break; + case 1: + motor[TIRE_FR].pwm = 40; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 60; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 40; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 3: + 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; + mtc=true; + break; + case 2: + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = FREE; + 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 = 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==1) { + motor[TIRE_FR].pwm = 5; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 5; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 5; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].pwm = 5; + motor[TIRE_BL].dir = FOR; + if(g[2]==0) { + mode=2; + } + } else if(mode==2) { + motor[TIRE_FR].pwm = 15; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].pwm = 15; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 15; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 15; + motor[TIRE_BL].dir = BACK; + mode1g0++; + if(mode1g0>10000&&g[2]==0) { + mode=3; + } + } else if(mode==3) { + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = BRAKE; + } else { + motor[TIRE_FR].pwm = 0; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].pwm = 0; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].pwm = 0; + motor[TIRE_BL].dir = BRAKE; + } + } #endif #if USE_PROCESS_NUM>3 -static void Process3() +static void Process3() { - } #endif #if USE_PROCESS_NUM>4 -static void Process4() -{ +static void Process4() +{ } #endif #if USE_PROCESS_NUM>5 -static void Process5() -{ - +static void Process5() +{ + } #endif #if USE_PROCESS_NUM>6 -static void Process6() +static void Process6() { - + } #endif #if USE_PROCESS_NUM>7 static void Process7() { - + } #endif -#if USE_PROCESS_NUM>8 +#if USE_PROCESS_NUM>8 static void Process8() { - + } #endif #if USE_PROCESS_NUM>9 static void Process9() { - + } #endif #endif @@ -318,25 +638,27 @@ static void AllActuatorReset() { - #ifdef USE_SOLENOID - solenoid.all = ALL_SOLENOID_OFF; - #endif +#ifdef USE_SOLENOID + solenoid.all = ALL_SOLENOID_OFF; +#endif - #ifdef USE_MOTOR - for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) - { - motor[i].dir = FREE; - motor[i].pwm = 0; - } - #endif +#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(){ +/* +void BuzzerTimer_func() +{ buzzer = !buzzer; //LED_DEBUG0 = !LED_DEBUG0; } - -void TapeLedEms_func() { +*/ +void TapeLedEms_func() +{ sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; }