From d9f8e4eb9ed3f54622d74ed580513f7e0aec6130 Mon Sep 17 00:00:00 2001 From: MRprusa3d Date: Thu, 17 Jan 2019 02:57:08 +0100 Subject: [PATCH] PID / manage_heater PSD regulator improvement --- Firmware/Configuration.h | 2 ++ Firmware/temperature.cpp | 73 ++++++++++++++++++++++------------------ Firmware/temperature.h | 8 ++++- 3 files changed, 49 insertions(+), 34 deletions(-) diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h index 3bed653b7..e4eb7e17f 100644 --- a/Firmware/Configuration.h +++ b/Firmware/Configuration.h @@ -131,8 +131,10 @@ //#define PID_DEBUG // Sends debug data to the serial port. //#define PID_OPENLOOP 1 // Puts PID in open loop. M104/M140 sets the output power from 0 to PID_MAX //#define SLOW_PWM_HEATERS // PWM with very low frequency (roughly 0.125Hz=8s) and minimum state time of approximately 1s useful for heaters driven by a relay +// :-O :-O :-O #define PID_FUNCTIONAL_RANGE 10 // If the temperature difference between the target temperature and the actual temperature // is more then PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max. +// :-O :-O :-O #define PID_INTEGRAL_DRIVE_MAX PID_MAX //limit for the integral term #define K1 0.95 //smoothing factor within the PID #define PID_dT ((OVERSAMPLENR * 10.0)/(F_CPU / 64.0 / 256.0)) //sampling period of the temperature routine diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp index b651c2737..e4315dd49 100644 --- a/Firmware/temperature.cpp +++ b/Firmware/temperature.cpp @@ -106,15 +106,15 @@ static volatile bool temp_meas_ready = false; #ifdef PIDTEMP //static cannot be external: - static float temp_iState[EXTRUDERS] = { 0 }; - static float temp_dState[EXTRUDERS] = { 0 }; + static float iState_sum[EXTRUDERS] = { 0 }; + static float dState_last[EXTRUDERS] = { 0 }; static float pTerm[EXTRUDERS]; static float iTerm[EXTRUDERS]; static float dTerm[EXTRUDERS]; //int output; static float pid_error[EXTRUDERS]; - static float temp_iState_min[EXTRUDERS]; - static float temp_iState_max[EXTRUDERS]; + static float iState_sum_min[EXTRUDERS]; + static float iState_sum_max[EXTRUDERS]; // static float pid_input[EXTRUDERS]; // static float pid_output[EXTRUDERS]; static bool pid_reset[EXTRUDERS]; @@ -418,7 +418,7 @@ void updatePID() { #ifdef PIDTEMP for(int e = 0; e < EXTRUDERS; e++) { - temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / cs.Ki; + iState_sum_max[e] = PID_INTEGRAL_DRIVE_MAX / cs.Ki; } #endif #ifdef PIDTEMPBED @@ -587,6 +587,10 @@ void checkExtruderAutoFans() #endif // any extruder auto fan pins set +void resetPID(uint8_t extruder) // ready for eventually parameters adjusting +{ +} + void manage_heater() { #ifdef WATCHDOG @@ -598,6 +602,7 @@ void manage_heater() if(temp_meas_ready != true) //better readability return; +// more precisely - this condition partially stabilizes time interval for regulation values evaluation (@ ~ 230ms) updateTemperaturesFromRawValues(); @@ -619,38 +624,42 @@ void manage_heater() pid_input = current_temperature[e]; #ifndef PID_OPENLOOP - pid_error[e] = target_temperature[e] - pid_input; - if(pid_error[e] > PID_FUNCTIONAL_RANGE) { - pid_output = BANG_MAX; - pid_reset[e] = true; - } - else if(pid_error[e] < -PID_FUNCTIONAL_RANGE || target_temperature[e] == 0) { + if(target_temperature[e] == 0) { pid_output = 0; pid_reset[e] = true; - } - else { - if(pid_reset[e] == true) { - temp_iState[e] = 0.0; + } else { + pid_error[e] = target_temperature[e] - pid_input; + if(pid_reset[e]) { + iState_sum[e] = 0.0; + dTerm[e] = 0.0; // 'dState_last[e]' initial setting is not necessary (see end of if-statement) pid_reset[e] = false; } +#ifndef PonM pTerm[e] = cs.Kp * pid_error[e]; - temp_iState[e] += pid_error[e]; - temp_iState[e] = constrain(temp_iState[e], temp_iState_min[e], temp_iState_max[e]); - iTerm[e] = cs.Ki * temp_iState[e]; - - //K1 defined in Configuration.h in the PID settings + iState_sum[e] += pid_error[e]; + iState_sum[e] = constrain(iState_sum[e], iState_sum_min[e], iState_sum_max[e]); + iTerm[e] = cs.Ki * iState_sum[e]; + // K1 defined in Configuration.h in the PID settings #define K2 (1.0-K1) - dTerm[e] = (cs.Kd * (pid_input - temp_dState[e]))*K2 + (K1 * dTerm[e]); - pid_output = pTerm[e] + iTerm[e] - dTerm[e]; + dTerm[e] = (cs.Kd * (pid_input - dState_last[e]))*K2 + (K1 * dTerm[e]); // e.g. digital filtration of derivative term changes + pid_output = pTerm[e] + iTerm[e] - dTerm[e]; // subtraction due to "Derivative on Measurement" method (i.e. derivative of input instead derivative of error is used) if (pid_output > PID_MAX) { - if (pid_error[e] > 0 ) temp_iState[e] -= pid_error[e]; // conditional un-integration + if (pid_error[e] > 0 ) iState_sum[e] -= pid_error[e]; // conditional un-integration pid_output=PID_MAX; - } else if (pid_output < 0){ - if (pid_error[e] < 0 ) temp_iState[e] -= pid_error[e]; // conditional un-integration + } else if (pid_output < 0) { + if (pid_error[e] < 0 ) iState_sum[e] -= pid_error[e]; // conditional un-integration pid_output=0; } +#else // PonM ("Proportional on Measurement" method) + iState_sum[e] += cs.Ki * pid_error[e]; + iState_sum[e] -= cs.Kp * (pid_input - dState_last[e]); + iState_sum[e] = constrain(iState_sum[e], 0, PID_INTEGRAL_DRIVE_MAX); + dTerm[e] = cs.Kd * (pid_input - dState_last[e]); + pid_output = iState_sum[e] - dTerm[e]; // subtraction due to "Derivative on Measurement" method (i.e. derivative of input instead derivative of error is used) + pid_output = constrain(pid_output, 0, PID_MAX); +#endif // PonM } - temp_dState[e] = pid_input; + dState_last[e] = pid_input; #else pid_output = constrain(target_temperature[e], 0, PID_MAX); #endif //PID_OPENLOOP @@ -667,7 +676,7 @@ void manage_heater() SERIAL_ECHO(" iTerm "); SERIAL_ECHO(iTerm[e]); SERIAL_ECHO(" dTerm "); - SERIAL_ECHOLN(dTerm[e]); + SERIAL_ECHOLN(-dTerm[e]); #endif //PID_DEBUG #else /* PID off */ pid_output = 0; @@ -677,16 +686,14 @@ void manage_heater() #endif // Check if temperature is within the correct range - if(current_temperature[e] < maxttemp[e]) + if((current_temperature[e] < maxttemp[e]) && (target_temperature[e] != 0)) { soft_pwm[e] = (int)pid_output >> 1; } else - { + { soft_pwm[e] = 0; } - if(target_temperature[e]==0) - soft_pwm[e] = 0; #ifdef WATCH_TEMP_PERIOD if(watchmillis[e] && millis() - watchmillis[e] > WATCH_TEMP_PERIOD) @@ -1001,8 +1008,8 @@ void tp_init() // populate with the first value maxttemp[e] = maxttemp[0]; #ifdef PIDTEMP - temp_iState_min[e] = 0.0; - temp_iState_max[e] = PID_INTEGRAL_DRIVE_MAX / cs.Ki; + iState_sum_min[e] = 0.0; + iState_sum_max[e] = PID_INTEGRAL_DRIVE_MAX / cs.Ki; #endif //PIDTEMP #ifdef PIDTEMPBED temp_iState_min_bed = 0.0; diff --git a/Firmware/temperature.h b/Firmware/temperature.h index 9a3cee834..8db41672e 100644 --- a/Firmware/temperature.h +++ b/Firmware/temperature.h @@ -87,6 +87,8 @@ extern int current_voltage_raw_bed; extern volatile int babystepsTodo[3]; #endif +void resetPID(uint8_t extruder); + inline void babystepsTodoZadd(int n) { if (n != 0) { @@ -137,11 +139,15 @@ FORCE_INLINE float degTargetBed() { FORCE_INLINE void setTargetHotend(const float &celsius, uint8_t extruder) { target_temperature[extruder] = celsius; + resetPID[extruder]; }; static inline void setTargetHotendSafe(const float &celsius, uint8_t extruder) { - if (extruder