From 16c1f0641e8971895cc9690991c4b3662e8b5988 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 28 Nov 2018 17:33:17 +0200 Subject: fix PID in heaters. waste tank valve in intersegment (bug #323) --- Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index bee00c049..68bd680a8 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -523,7 +523,7 @@ uint32_t MillisecLoop(uint32_t tick) } //ROM_IntMasterEnable(); - //loop_Run_Read_Speed(); - just for testing + return OK; } /****************************************************************************** -- cgit v1.3.1 From f8416707278aabcebcccbbe4e1a8a9b5d66e0f82 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Thu, 29 Nov 2018 14:58:05 +0200 Subject: advanced PID in thread and heaters - use only with modified congiguration --- .../Embedded_SW/Embedded/Modules/Control/PIDAlgo.c | 36 ++++++++++++++++++++++ .../Embedded_SW/Embedded/Modules/Control/PIDAlgo.h | 4 +++ .../Embedded/Modules/General/GeneralHardware.c | 1 + .../Embedded/Modules/Heaters/Heaters_print.c | 10 ++++-- .../Embedded/Modules/Thread/Thread_print.c | 17 +++++----- 5 files changed, 58 insertions(+), 10 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Control') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c index f37759943..72774461a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c @@ -36,4 +36,40 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P return output; } +float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral) +{ + float error; + float derivative; + float output; + + //Calculate P,I,D + error = _setPoint - _mesuredParam; + + //In case of error too small then stop integration + if(abs(error) > params->epsilon) + { + *_integral = *_integral + error*params->dt; + } + + derivative = (error - *_pre_error)/params->dt; + float IntegralErrorMultiplier; + float ProportionalErrorMultiplier; + + output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative; + + //Saturation Filter + if(output > params->MAX) + { + output = params->MAX; + } + else if(output < params->MIN) + { + output = params->MIN; + } + + //Update error + *_pre_error = error; + + return output; +} diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h index 03c363131..4792be9ec 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h +++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h @@ -12,7 +12,11 @@ typedef struct float Kp; float Kd; float Ki; + float IntegralErrorMultiplier; + float ProportionalErrorMultiplier; + }PID_Config_Params; float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral); +float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral); #endif /* MODULES_PIDALGO_H_ */ diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c index a0ac11d1f..4ad9af903 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c +++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c @@ -108,6 +108,7 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest) { BlowerCfg.enabled = true; BlowerCfg.voltage = request->blowers[0]->voltage; + BlowerCfg.heatingvoltage = request->blowers[0]->heatingvoltage; /*Turn_the_Blower_On();//Turn on with the Default_Voltage if (request->blowers[0]->voltage) Control_Voltage_To_Blower(request->blowers[0]->voltage); diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c index aa3d695de..4f8edde02 100644 --- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c @@ -145,7 +145,9 @@ void LoadHeaterState(HeaterType HeaterType,HeaterState *HeaterState) HeaterState->has_heatertype = true; HeaterState->heatertype = HeaterType; HeaterState->has_setpoint = true; - HeaterState->setpoint = HeaterCmd[HeaterId].targettemperatue/100; + // HeaterState->setpoint = HeaterCmd[HeaterId].targettemperatue/100; + // if (HeaterType >= HEATER_TYPE__HeaterZone1) + HeaterState->setpoint = DCTimeSliceAllocation[HeaterId]*100/NumberOFSlicesInUse; HeaterState->has_currentvalue = true; HeaterState->currentvalue = TemperatureSensorRead(HeaterId2PT100Id[HeaterId])/100; HeaterState->has_isactive = true; @@ -359,7 +361,9 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue) if (BlowerCfg.enabled == true) { Turn_the_Blower_On();//Turn on with the Default_Voltage - if (BlowerCfg.voltage) + if (BlowerCfg.heatingvoltage) + Control_Voltage_To_Blower(BlowerCfg.heatingvoltage); + else Control_Voltage_To_Blower(BlowerCfg.voltage-500); } @@ -653,7 +657,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue) // Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index, Counter[index]); // #warning PID is now only proportional (above) if (printindex==index) - Report(heatstr,__FILE__,__LINE__,index,RpWarning,readValue, HeaterPIDConfig[index].m_calculatedError); + Report(heatstr,__FILE__,__LINE__,index,RpWarning,HeaterPIDConfig[index].m_calculatedError,0); HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError/100)); } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 5390ed0a7..15ea5157b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -319,7 +319,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) #define MAX_THREAD_CONTROL_LOG 300 double calculatedError[MAX_THREAD_CONTROL_LOG+1]; double NormError[MAX_THREAD_CONTROL_LOG+1]; -double Integral[MAX_THREAD_CONTROL_LOG+1]; +double mIntegral[MAX_THREAD_CONTROL_LOG+1]; int MotorId[MAX_THREAD_CONTROL_LOG+1]; int readValue[MAX_THREAD_CONTROL_LOG+1]; int AveragereadValue[MAX_THREAD_CONTROL_LOG+1]; @@ -452,13 +452,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //EndState(CurrentJob,Message ); SegmentReady(Module_Thread,ModuleFail); AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,true); - LOG_ERROR (index, "Dancer Failure"); + LOG_ERROR (DancerId, "Dancer Failure"); return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; DancerError[DancerId] = NormalizedError; - MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, + MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit { @@ -478,9 +478,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) readValue[controlIndex] = ReadValue; AveragereadValue[controlIndex] = avreageSampleValue; calculatedspeed[controlIndex] = calculated_speed; + NormError[controlIndex] + = MotorControlConfig[index].m_mesuredParam; + mIntegral[controlIndex] = MotorControlConfig[index].m_integral; timestamp[controlIndex] = msec_millisecondCounter; - NormError[controlIndex]= NormalizedError; - Integral[controlIndex] = MotorControlConfig[index].m_integral; if (controlIndex++>=MAX_THREAD_CONTROL_LOG) controlIndex = 0; } @@ -555,8 +556,10 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; - MotorControlConfig[Motor_i].m_params.epsilon = 0.1; - MotorControlConfig[Motor_i].m_params.dt = 1000; + MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp; + MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue; + MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon; + MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype; MotorControlConfig[Motor_i].m_calculatedError = 0; MotorControlConfig[Motor_i].m_integral = 0; MotorControlConfig[Motor_i].m_isEnabled = true; -- cgit v1.3.1