From 1db7013e3be036381df249d6b482e20aed31c811 Mon Sep 17 00:00:00 2001 From: mikeshub Date: Sat, 22 Jun 2019 16:52:56 -0500 Subject: [PATCH] Fix and improve PID loops (#14373) - Windup guarding was missing. The kludge in place of windup guard is removed. D term filter calculations are simplified to require fewer `float` calculations. Sign change for D term output to make debugging output clearer. - Use "no overshoot" for bed PID tuning. --- Marlin/src/module/temperature.cpp | 83 +++++++++++++++---------------- 1 file changed, 39 insertions(+), 44 deletions(-) diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index a496748581..23f2d003da 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -350,11 +350,13 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 PID_t tune_pid = { 0, 0, 0 }; float max = 0, min = 10000; + const bool isbed = (heater < 0); + #if HAS_PID_FOR_BOTH - #define GHV(B,H) (heater < 0 ? (B) : (H)) - #define SHV(B,H) do{ if (heater < 0) temp_bed.soft_pwm_amount = B; else temp_hotend[heater].soft_pwm_amount = H; }while(0) - #define ONHEATINGSTART() (heater < 0 ? printerEventLEDs.onBedHeatingStart() : printerEventLEDs.onHotendHeatingStart()) - #define ONHEATING(S,C,T) do{ if (heater < 0) printerEventLEDs.onBedHeating(S,C,T); else printerEventLEDs.onHotendHeating(S,C,T); }while(0) + #define GHV(B,H) (isbed ? (B) : (H)) + #define SHV(B,H) do{ if (isbed) temp_bed.soft_pwm_amount = B; else temp_hotend[heater].soft_pwm_amount = H; }while(0) + #define ONHEATINGSTART() (isbed ? printerEventLEDs.onBedHeatingStart() : printerEventLEDs.onHotendHeatingStart()) + #define ONHEATING(S,C,T) (isbed ? printerEventLEDs.onBedHeating(S,C,T) : printerEventLEDs.onHotendHeating(S,C,T)) #elif ENABLED(PIDTEMPBED) #define GHV(B,H) B #define SHV(B,H) (temp_bed.soft_pwm_amount = B) @@ -370,7 +372,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 #if WATCH_BED || WATCH_HOTENDS #define HAS_TP_BED BOTH(THERMAL_PROTECTION_BED, PIDTEMPBED) #if HAS_TP_BED && BOTH(THERMAL_PROTECTION_HOTENDS, PIDTEMP) - #define GTV(B,H) (heater < 0 ? (B) : (H)) + #define GTV(B,H) (isbed ? (B) : (H)) #elif HAS_TP_BED #define GTV(B,H) (B) #else @@ -456,23 +458,25 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 SERIAL_ECHOPAIR(MSG_BIAS, bias, MSG_D, d, MSG_T_MIN, min, MSG_T_MAX, max); if (cycles > 2) { - float Ku = (4.0f * d) / (float(M_PI) * (max - min) * 0.5f), - Tu = ((float)(t_low + t_high) * 0.001f); - tune_pid.Kp = 0.6f * Ku; + const float Ku = (4.0f * d) / (float(M_PI) * (max - min) * 0.5f), + Tu = float(t_low + t_high) * 0.001f, + pf = isbed ? 0.2f : 0.6f, + df = isbed ? 1.0f / 3.0f : 1.0f / 8.0f; + tune_pid.Kp = Ku * pf; + tune_pid.Kd = tune_pid.Kp * Tu * df; tune_pid.Ki = 2 * tune_pid.Kp / Tu; - tune_pid.Kd = tune_pid.Kp * Tu * 0.125f; SERIAL_ECHOPAIR(MSG_KU, Ku, MSG_TU, Tu); SERIAL_ECHOLNPGM("\n" MSG_CLASSIC_PID); SERIAL_ECHOLNPAIR(MSG_KP, tune_pid.Kp, MSG_KI, tune_pid.Ki, MSG_KD, tune_pid.Kd); /** - tune_pid.Kp = 0.33*Ku; - tune_pid.Ki = tune_pid.Kp/Tu; - tune_pid.Kd = tune_pid.Kp*Tu/3; + tune_pid.Kp = 0.33 * Ku; + tune_pid.Ki = tune_pid.Kp / Tu; + tune_pid.Kd = tune_pid.Kp * Tu / 3; SERIAL_ECHOLNPGM(" Some overshoot"); SERIAL_ECHOLNPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd, " No overshoot"); - tune_pid.Kp = 0.2*Ku; - tune_pid.Ki = 2*tune_pid.Kp/Tu; - tune_pid.Kd = tune_pid.Kp*Tu/3; + tune_pid.Kp = 0.2 * Ku; + tune_pid.Ki = 2 * tune_pid.Kp / Tu; + tune_pid.Kd = tune_pid.Kp * Tu / 3; SERIAL_ECHOPAIR(" Kp: ", tune_pid.Kp, " Ki: ", tune_pid.Ki, " Kd: ", tune_pid.Kd); */ } @@ -496,7 +500,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 // Report heater states every 2 seconds if (ELAPSED(ms, next_temp_ms)) { #if HAS_TEMP_SENSOR - print_heater_states(heater >= 0 ? heater : active_extruder); + print_heater_states(isbed ? active_extruder : heater); SERIAL_EOL(); #endif next_temp_ms = ms + 2000UL; @@ -507,9 +511,9 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 #if WATCH_BED && WATCH_HOTENDS true #elif WATCH_HOTENDS - heater >= 0 + !isbed #else - heater < 0 + isbed #endif ) { if (!heated) { // If not yet reached target... @@ -569,7 +573,7 @@ temp_range_t Temperature::temp_range[HOTENDS] = ARRAY_BY_HOTENDS(sensor_heater_0 // Use the result? (As with "M303 U1") if (set_result) { #if HAS_PID_FOR_BOTH - if (heater < 0) _SET_BED_PID(); else _SET_EXTRUDER_PID(); + if (isbed) _SET_BED_PID(); else _SET_EXTRUDER_PID(); #elif ENABLED(PIDTEMP) _SET_EXTRUDER_PID(); #else @@ -805,9 +809,7 @@ float Temperature::get_pid_output(const int8_t e) { static float temp_iState[HOTENDS] = { 0 }, temp_dState[HOTENDS] = { 0 }; static bool pid_reset[HOTENDS] = { false }; - float pid_error = temp_hotend[HOTEND_INDEX].target - temp_hotend[HOTEND_INDEX].current; - work_pid[HOTEND_INDEX].Kd = PID_K2 * PID_PARAM(Kd, HOTEND_INDEX) * (temp_hotend[HOTEND_INDEX].current - temp_dState[HOTEND_INDEX]) + float(PID_K1) * work_pid[HOTEND_INDEX].Kd; - temp_dState[HOTEND_INDEX] = temp_hotend[HOTEND_INDEX].current; + const float pid_error = temp_hotend[HOTEND_INDEX].target - temp_hotend[HOTEND_INDEX].current; if (temp_hotend[HOTEND_INDEX].target == 0 || pid_error < -(PID_FUNCTIONAL_RANGE) @@ -825,13 +827,17 @@ float Temperature::get_pid_output(const int8_t e) { else { if (pid_reset[HOTEND_INDEX]) { temp_iState[HOTEND_INDEX] = 0.0; + work_pid[HOTEND_INDEX].Kd = 0.0; pid_reset[HOTEND_INDEX] = false; } - temp_iState[HOTEND_INDEX] += pid_error; + + work_pid[HOTEND_INDEX].Kd = work_pid[HOTEND_INDEX].Kd + PID_K2 * (PID_PARAM(Kd, HOTEND_INDEX) * (temp_dState[HOTEND_INDEX] - temp_hotend[HOTEND_INDEX].current) - work_pid[HOTEND_INDEX].Kd); + const float max_power_over_i_gain = (float)PID_MAX / PID_PARAM(Ki, HOTEND_INDEX); + temp_iState[HOTEND_INDEX] = constrain(temp_iState[HOTEND_INDEX] + pid_error, 0, max_power_over_i_gain); work_pid[HOTEND_INDEX].Kp = PID_PARAM(Kp, HOTEND_INDEX) * pid_error; work_pid[HOTEND_INDEX].Ki = PID_PARAM(Ki, HOTEND_INDEX) * temp_iState[HOTEND_INDEX]; - pid_output = work_pid[HOTEND_INDEX].Kp + work_pid[HOTEND_INDEX].Ki - work_pid[HOTEND_INDEX].Kd; + pid_output = work_pid[HOTEND_INDEX].Kp + work_pid[HOTEND_INDEX].Ki + work_pid[HOTEND_INDEX].Kd; #if ENABLED(PID_EXTRUSION_SCALING) work_pid[HOTEND_INDEX].Kc = 0; @@ -850,15 +856,9 @@ float Temperature::get_pid_output(const int8_t e) { } #endif // PID_EXTRUSION_SCALING - if (pid_output > PID_MAX) { - if (pid_error > 0) temp_iState[HOTEND_INDEX] -= pid_error; // conditional un-integration - pid_output = PID_MAX; - } - else if (pid_output < 0) { - if (pid_error < 0) temp_iState[HOTEND_INDEX] -= pid_error; // conditional un-integration - pid_output = 0; - } + pid_output = constrain(pid_output, 0, PID_MAX); } + temp_dState[HOTEND_INDEX] = temp_hotend[HOTEND_INDEX].current; #else // PID_OPENLOOP @@ -908,23 +908,18 @@ float Temperature::get_pid_output(const int8_t e) { static PID_t work_pid = { 0 }; static float temp_iState = 0, temp_dState = 0; - float pid_error = temp_bed.target - temp_bed.current; - temp_iState += pid_error; + const float max_power_over_i_gain = (float)MAX_BED_POWER / temp_bed.pid.Ki, + pid_error = temp_bed.target - temp_bed.current; + + temp_iState = constrain(temp_iState + pid_error, 0, max_power_over_i_gain); + work_pid.Kp = temp_bed.pid.Kp * pid_error; work_pid.Ki = temp_bed.pid.Ki * temp_iState; - work_pid.Kd = PID_K2 * temp_bed.pid.Kd * (temp_bed.current - temp_dState) + PID_K1 * work_pid.Kd; + work_pid.Kd = work_pid.Kd + PID_K2 * (temp_bed.pid.Kd * (temp_dState - temp_bed.current) - work_pid.Kd); temp_dState = temp_bed.current; - float pid_output = work_pid.Kp + work_pid.Ki - work_pid.Kd; - if (pid_output > MAX_BED_POWER) { - if (pid_error > 0) temp_iState -= pid_error; // conditional un-integration - pid_output = MAX_BED_POWER; - } - else if (pid_output < 0) { - if (pid_error < 0) temp_iState -= pid_error; // conditional un-integration - pid_output = 0; - } + const float pid_output = constrain(work_pid.Kp + work_pid.Ki + work_pid.Kd, 0, MAX_BED_POWER); #else // PID_OPENLOOP