Dans98 klipper velocity pid (#47)

* added support for velocity pid control algorithm Signed-off-by: Daniel Sherman <[email protected]> Signed-off-by: Vinzenz Hassert <[email protected]> * formatting --------- Signed-off-by: Daniel Sherman <[email protected]> Signed-off-by: Vinzenz Hassert <[email protected]> Co-authored-by: Daniel <[email protected]>
KalicoCrew · Aug 26, 2023 · e5fc1d4 · e5fc1d4
1 parent c2a9a5f
commit e5fc1d4
Show file tree

Hide file tree

Showing 4 changed files with 87 additions and 4 deletions.
diff --git a/.gitignore b/.gitignore
@@ -6,3 +6,5 @@ out
 klippy/.version
 .history/
 .DS_Store
+ci_build/
+ci_cache/
diff --git 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
@@ -51,7 +51,11 @@ 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}
+        algos = {
+            "watermark": ControlBangBang,
+            "pid": ControlPID,
+            "pid_v": ControlVelocityPID,
+        }
         algo = config.getchoice("control", algos)
         self.control = algo(self, config)
         # Setup output heater pin
@@ -228,6 +232,9 @@ def temperature_update(self, read_time, temp, target_temp):
     def check_busy(self, eventtime, smoothed_temp, target_temp):
         return smoothed_temp < target_temp - self.max_delta
 
+    def get_type(self):
+        return "watermark"
+
 
 ######################################################################
 # Proportional Integral Derivative (PID) control algo
@@ -288,6 +295,78 @@ def check_busy(self, eventtime, smoothed_temp, target_temp):
             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.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
+        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.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.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
+        i = self.Ki * error
+        d = self.Kd * -self.d2  # invert sign to prevent derivative kick
+
+        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
+        )
+
+    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
@@ -51,7 +51,8 @@ def cmd_PID_CALIBRATE(self, gcmd):
         )
         # Store results for SAVE_CONFIG
         configfile = self.printer.lookup_object("configfile")
-        configfile.set(heater_name, "control", "pid")
+        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,))