Klipper velocity pid

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:

klippy/extras/heaters.py

@@ -43,7 +43,11 @@ def __init__(self, config, sensor):
         self.next_pwm_time = 0.
         self.last_pwm_value = 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
@@ -163,7 +167,8 @@ def temperature_update(self, read_time, temp, target_temp):
             self.heater.set_pwm(read_time, 0.)
     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
@@ -216,7 +221,73 @@ 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)
+    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

klippy/extras/pid_calibrate.py

@@ -44,7 +44,8 @@ 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')
+        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,))