From 04e190b2e908c3eea7054d9ece5aa1f197442e84 Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 26 Jun 2023 18:36:36 -0500 Subject: [PATCH 1/2] added support for velocity pid control algorithm Signed-off-by: Daniel Sherman Signed-off-by: Vinzenz Hassert --- docs/Config_Reference.md | 5 +- klippy/extras/heaters.py | 85 ++++++++++++++++++++++++++++++---- klippy/extras/pid_calibrate.py | 12 ++--- 3 files changed, 86 insertions(+), 16 deletions(-) diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index d89b15f17..f609661f0 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -803,8 +803,9 @@ sensor_pin: # be smoothed to reduce the impact of measurement noise. The default # is 1 seconds. control: -# Control algorithm (either pid or watermark). This parameter must -# be provided. +# Control algorithm (either pid, pid_v or watermark). This parameter must +# be provided. pid_v should only be used on well calibrated heaters with +# low to moderate noise. pid_Kp: pid_Ki: pid_Kd: diff --git a/klippy/extras/heaters.py b/klippy/extras/heaters.py index 10fe829af..e5f1d89fb 100644 --- a/klippy/extras/heaters.py +++ b/klippy/extras/heaters.py @@ -51,8 +51,12 @@ def __init__(self, config, sensor): self.next_pwm_time = 0.0 self.last_pwm_value = 0.0 # Setup control algorithm sub-class - algos = {"watermark": ControlBangBang, "pid": ControlPID} - algo = config.getchoice("control", algos) + algos = { + 'watermark': ControlBangBang, + 'pid': ControlPID, + 'pid_v': ControlVelocityPID + } + algo = config.getchoice('control', algos) self.control = algo(self, config) # Setup output heater pin heater_pin = config.get("heater_pin") @@ -226,8 +230,9 @@ def temperature_update(self, read_time, temp, target_temp): self.heater.set_pwm(read_time, 0.0) def check_busy(self, eventtime, smoothed_temp, target_temp): - return smoothed_temp < target_temp - self.max_delta - + return smoothed_temp < target_temp-self.max_delta + def get_type(self): + return 'watermark' ###################################################################### # Proportional Integral Derivative (PID) control algo @@ -283,11 +288,75 @@ def temperature_update(self, read_time, temp, target_temp): def check_busy(self, eventtime, smoothed_temp, target_temp): temp_diff = target_temp - smoothed_temp - return ( - abs(temp_diff) > PID_SETTLE_DELTA - or abs(self.prev_temp_deriv) > PID_SETTLE_SLOPE - ) + return (abs(temp_diff) > PID_SETTLE_DELTA + or abs(self.prev_temp_deriv) > PID_SETTLE_SLOPE) + def get_type(self): + return 'pid' + +###################################################################### +# Velocity (PID) control algo +###################################################################### + +class ControlVelocityPID: + def __init__(self, heater, config): + self.heater = heater + self.heater_max_power = heater.get_max_power() + self.Kp = config.getfloat('pid_Kp') / PID_PARAM_BASE + self.Ki = config.getfloat('pid_Ki') / PID_PARAM_BASE + self.Kd = config.getfloat('pid_Kd') / PID_PARAM_BASE + self.smooth_time = heater.get_smooth_time() # smoothing window + self.temps = [AMBIENT_TEMP] * 3 # temperature readings + self.times = [0.] * 3 #temperature reading times + self.d1 = 0. # previous smoothed 1st derivative + self.d2 = 0. # previous smoothed 2nd derivative + self.pwm = 0. # the previous pwm setting + def temperature_update(self, read_time, temp, target_temp): + # update the temp and time lists + self.temps.pop(0) + self.temps.append(temp) + self.times.pop(0) + self.times.append(read_time) + + # calculate the 1st derivative: p part in velocity form + # note the derivative is of the temp and not the error + # this is to prevent derivative kick + d1 = self.temps[-1] - self.temps[-2] + + # calculate the error : i part in velocity form + error = self.times[-1] - self.times[-2] + error = error * (target_temp - self.temps[-1]) + + # calculate the 2nd derivative: d part in velocity form + # note the derivative is of the temp and not the error + # this is to prevent derivative kick + d2 = self.temps[-1] - 2.*self.temps[-2] + self.temps[-3] + d2 = d2 / (self.times[-1] - self.times[-2]) + + # smooth both the derivatives using a modified moving average + # that handles unevenly spaced data points + n = max(1.,self.smooth_time/(self.times[-1] - self.times[-2])) + self.d1 = ((n - 1.) * self.d1 + d1) / n + self.d2 = ((n - 1.) * self.d2 + d2) / n + + # calculate the output + p = self.Kp * -self.d1 # invert sign to prevent derivative kick + i = self.Ki * error + d = self.Kd * -self.d2 # invert sign to prevent derivative kick + + self.pwm = max(0., min(self.heater_max_power, self.pwm + p + i + d)) + if target_temp == 0.: + self.pwm = 0. + + # update the heater + self.heater.set_pwm(read_time, self.pwm) + + def check_busy(self, eventtime, smoothed_temp, target_temp): + temp_diff = target_temp - smoothed_temp + return (abs(temp_diff) > PID_SETTLE_DELTA + or abs(self.d1) > PID_SETTLE_SLOPE) + def get_type(self): + return 'pid_v' ###################################################################### # Sensor and heater lookup diff --git a/klippy/extras/pid_calibrate.py b/klippy/extras/pid_calibrate.py index d6b6fda05..3643dd67b 100644 --- a/klippy/extras/pid_calibrate.py +++ b/klippy/extras/pid_calibrate.py @@ -50,12 +50,12 @@ def cmd_PID_CALIBRATE(self, gcmd): "with these parameters and restart the printer." % (Kp, Ki, Kd) ) # Store results for SAVE_CONFIG - configfile = self.printer.lookup_object("configfile") - configfile.set(heater_name, "control", "pid") - configfile.set(heater_name, "pid_Kp", "%.3f" % (Kp,)) - configfile.set(heater_name, "pid_Ki", "%.3f" % (Ki,)) - configfile.set(heater_name, "pid_Kd", "%.3f" % (Kd,)) - + configfile = self.printer.lookup_object('configfile') + control = 'pid_v' if old_control.get_type() == 'pid_v' else 'pid' + configfile.set(heater_name, 'control', control) + configfile.set(heater_name, 'pid_Kp', "%.3f" % (Kp,)) + configfile.set(heater_name, 'pid_Ki', "%.3f" % (Ki,)) + configfile.set(heater_name, 'pid_Kd', "%.3f" % (Kd,)) TUNE_PID_DELTA = 5.0 From 2f2a79fb71294e2d3b16fa58109cfc3c1d3b903d Mon Sep 17 00:00:00 2001 From: Brandon Nance Date: Wed, 23 Aug 2023 17:22:35 -0400 Subject: [PATCH 2/2] formatting --- .gitignore | 2 + klippy/extras/heaters.py | 70 +++++++++++++++++++--------------- klippy/extras/pid_calibrate.py | 13 ++++--- 3 files changed, 49 insertions(+), 36 deletions(-) diff --git a/.gitignore b/.gitignore index 34eb1bf4c..45735517a 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,5 @@ out klippy/.version .history/ .DS_Store +ci_build/ +ci_cache/ diff --git a/klippy/extras/heaters.py b/klippy/extras/heaters.py index e5f1d89fb..7d8c9bef6 100644 --- a/klippy/extras/heaters.py +++ b/klippy/extras/heaters.py @@ -52,11 +52,11 @@ def __init__(self, config, sensor): self.last_pwm_value = 0.0 # Setup control algorithm sub-class algos = { - 'watermark': ControlBangBang, - 'pid': ControlPID, - 'pid_v': ControlVelocityPID + "watermark": ControlBangBang, + "pid": ControlPID, + "pid_v": ControlVelocityPID, } - algo = config.getchoice('control', algos) + algo = config.getchoice("control", algos) self.control = algo(self, config) # Setup output heater pin heater_pin = config.get("heater_pin") @@ -230,9 +230,11 @@ def temperature_update(self, read_time, temp, target_temp): self.heater.set_pwm(read_time, 0.0) def check_busy(self, eventtime, smoothed_temp, target_temp): - return smoothed_temp < target_temp-self.max_delta + return smoothed_temp < target_temp - self.max_delta + def get_type(self): - return 'watermark' + return "watermark" + ###################################################################### # Proportional Integral Derivative (PID) control algo @@ -288,28 +290,33 @@ def temperature_update(self, read_time, temp, target_temp): def check_busy(self, eventtime, smoothed_temp, target_temp): temp_diff = target_temp - smoothed_temp - return (abs(temp_diff) > PID_SETTLE_DELTA - or abs(self.prev_temp_deriv) > PID_SETTLE_SLOPE) + return ( + abs(temp_diff) > PID_SETTLE_DELTA + or abs(self.prev_temp_deriv) > PID_SETTLE_SLOPE + ) + def get_type(self): - return 'pid' + return "pid" + ###################################################################### # Velocity (PID) control algo ###################################################################### + class ControlVelocityPID: def __init__(self, heater, config): self.heater = heater self.heater_max_power = heater.get_max_power() - self.Kp = config.getfloat('pid_Kp') / PID_PARAM_BASE - self.Ki = config.getfloat('pid_Ki') / PID_PARAM_BASE - self.Kd = config.getfloat('pid_Kd') / PID_PARAM_BASE - self.smooth_time = heater.get_smooth_time() # smoothing window - self.temps = [AMBIENT_TEMP] * 3 # temperature readings - self.times = [0.] * 3 #temperature reading times - self.d1 = 0. # previous smoothed 1st derivative - self.d2 = 0. # previous smoothed 2nd derivative - self.pwm = 0. # the previous pwm setting + self.Kp = config.getfloat("pid_Kp") / PID_PARAM_BASE + self.Ki = config.getfloat("pid_Ki") / PID_PARAM_BASE + self.Kd = config.getfloat("pid_Kd") / PID_PARAM_BASE + self.smooth_time = heater.get_smooth_time() # smoothing window + self.temps = [AMBIENT_TEMP] * 3 # temperature readings + self.times = [0.0] * 3 # temperature reading times + self.d1 = 0.0 # previous smoothed 1st derivative + self.d2 = 0.0 # previous smoothed 2nd derivative + self.pwm = 0.0 # the previous pwm setting def temperature_update(self, read_time, temp, target_temp): # update the temp and time lists @@ -330,33 +337,36 @@ def temperature_update(self, read_time, temp, target_temp): # calculate the 2nd derivative: d part in velocity form # note the derivative is of the temp and not the error # this is to prevent derivative kick - d2 = self.temps[-1] - 2.*self.temps[-2] + self.temps[-3] + d2 = self.temps[-1] - 2.0 * self.temps[-2] + self.temps[-3] d2 = d2 / (self.times[-1] - self.times[-2]) # smooth both the derivatives using a modified moving average # that handles unevenly spaced data points - n = max(1.,self.smooth_time/(self.times[-1] - self.times[-2])) - self.d1 = ((n - 1.) * self.d1 + d1) / n - self.d2 = ((n - 1.) * self.d2 + d2) / n + n = max(1.0, self.smooth_time / (self.times[-1] - self.times[-2])) + self.d1 = ((n - 1.0) * self.d1 + d1) / n + self.d2 = ((n - 1.0) * self.d2 + d2) / n # calculate the output - p = self.Kp * -self.d1 # invert sign to prevent derivative kick + p = self.Kp * -self.d1 # invert sign to prevent derivative kick i = self.Ki * error - d = self.Kd * -self.d2 # invert sign to prevent derivative kick + d = self.Kd * -self.d2 # invert sign to prevent derivative kick - self.pwm = max(0., min(self.heater_max_power, self.pwm + p + i + d)) - if target_temp == 0.: - self.pwm = 0. + self.pwm = max(0.0, min(self.heater_max_power, self.pwm + p + i + d)) + if target_temp == 0.0: + self.pwm = 0.0 # update the heater self.heater.set_pwm(read_time, self.pwm) def check_busy(self, eventtime, smoothed_temp, target_temp): temp_diff = target_temp - smoothed_temp - return (abs(temp_diff) > PID_SETTLE_DELTA - or abs(self.d1) > PID_SETTLE_SLOPE) + return ( + abs(temp_diff) > PID_SETTLE_DELTA or abs(self.d1) > PID_SETTLE_SLOPE + ) + def get_type(self): - return 'pid_v' + return "pid_v" + ###################################################################### # Sensor and heater lookup diff --git a/klippy/extras/pid_calibrate.py b/klippy/extras/pid_calibrate.py index 3643dd67b..994bb32da 100644 --- a/klippy/extras/pid_calibrate.py +++ b/klippy/extras/pid_calibrate.py @@ -50,12 +50,13 @@ def cmd_PID_CALIBRATE(self, gcmd): "with these parameters and restart the printer." % (Kp, Ki, Kd) ) # Store results for SAVE_CONFIG - configfile = self.printer.lookup_object('configfile') - control = 'pid_v' if old_control.get_type() == 'pid_v' else 'pid' - configfile.set(heater_name, 'control', control) - configfile.set(heater_name, 'pid_Kp', "%.3f" % (Kp,)) - configfile.set(heater_name, 'pid_Ki', "%.3f" % (Ki,)) - configfile.set(heater_name, 'pid_Kd', "%.3f" % (Kd,)) + configfile = self.printer.lookup_object("configfile") + control = "pid_v" if old_control.get_type() == "pid_v" else "pid" + configfile.set(heater_name, "control", control) + configfile.set(heater_name, "pid_Kp", "%.3f" % (Kp,)) + configfile.set(heater_name, "pid_Ki", "%.3f" % (Ki,)) + configfile.set(heater_name, "pid_Kd", "%.3f" % (Kd,)) + TUNE_PID_DELTA = 5.0