Dual loop pid

README.md

@@ -22,6 +22,8 @@ If I want my printer to light itself on fire, I should be able to make my printe
 
 - [heater: velocity PID](https://github.com/DangerKlippers/danger-klipper/pull/47) ([klipper#6272](https://github.com/Klipper3d/klipper/pull/6272))
 
+- [heater: Dual Loop PID](https://github.com/DangerKlippers/danger-klipper/issues/58) ([klipper#5972](https://github.com/Klipper3d/klipper/pull/5972))
+
 - [gcode: jinja2.ext.do extension](https://github.com/DangerKlippers/danger-klipper/pull/26) ([klipper#5149](https://github.com/Klipper3d/klipper/pull/5149))
 
 - [gcode: gcode_shell_command](https://github.com/DangerKlippers/danger-klipper/pull/26) ([klipper#2173](https://github.com/Klipper3d/klipper/pull/2173) / [kiuah](https://github.com/dw-0/kiauh/blob/master/resources/gcode_shell_command.py) )

docs/Config_Reference.md

@@ -940,6 +940,13 @@ sensor_type:
 sensor_pin:
 #   Analog input pin connected to the sensor. This parameter must be
 #   provided.
+#secondary_sensor_name:
+#   The temperature_sensor name of a second sensor to use for temperature
+#   control. This sensor will limit the heater power to not allow the
+#   temperature to exceed the 'secondary_max_temp_target' value.
+#   This sensor will work alongside the dual_loop_pid control algorithm.
+#secondary_max_temp_target:
+#   The maximum temperature target that the secondary sensor will allow.
 #pullup_resistor: 4700
 #   The resistance (in ohms) of the pullup attached to the thermistor.
 #   This parameter is only valid when the sensor is a thermistor. The
@@ -949,9 +956,9 @@ sensor_pin:
 #   be smoothed to reduce the impact of measurement noise. The default
 #   is 1 seconds.
 control:
-#   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.
+#   Control algorithm (either pid, pid_v, dual_loop_pid 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:
@@ -964,6 +971,23 @@ pid_Kd:
 #   off and 1.0 being full on. Consider using the PID_CALIBRATE
 #   command to obtain these parameters. The pid_Kp, pid_Ki, and pid_Kd
 #   parameters must be provided for PID heaters.
+#primary_pid_kp:
+#primary_pid_ki:
+#primary_pid_kd:
+#secondary_pid_kp:
+#secondary_pid_ki:
+#secondary_pid_kd:
+#   On 'dual_loop_pid' control use two PID loops to control the
+#   temperature. The primary PID loop controls the temperature
+#   directly. The secondary PID loop controls the power to the primary
+#   PID loop. This allows the primary PID loop to be tuned for
+#   temperature control, while the secondary PID loop can be tuned for
+#   power control, not exceeding the temperature limit set on
+#   'secondary_max_temp_target'.
+#   The primary sensor is positioned close where the temperature
+#   measurament should be more accurate (e.g. on the bed surface). The
+#   secondary sensor is positioned where the temperature measurament
+#   should not exceed a limit (e.g. on the silicone heater).
 #max_delta: 2.0
 #   On 'watermark' controlled heaters this is the number of degrees in
 #   Celsius above the target temperature before disabling the heater

klippy/extras/heaters.py

@@ -18,7 +18,7 @@
 
 
 class Heater:
-    def __init__(self, config, sensor):
+    def __init__(self, config, sensor, secondary_sensor=None):
         self.printer = config.get_printer()
         self.name = config.get_name().split()[-1]
         # Setup sensor
@@ -28,6 +28,7 @@ def __init__(self, config, sensor):
         self.sensor.setup_minmax(self.min_temp, self.max_temp)
         self.sensor.setup_callback(self.temperature_callback)
         self.pwm_delay = self.sensor.get_report_time_delta()
+        self.secondary_sensor = secondary_sensor
         # Setup temperature checks
         self.min_extrude_temp = config.getfloat(
             "min_extrude_temp",
@@ -55,9 +56,15 @@ def __init__(self, config, sensor):
             "watermark": ControlBangBang,
             "pid": ControlPID,
             "pid_v": ControlVelocityPID,
+            "dual_loop_pid": ControlDualLoopPID,
         }
         algo = config.getchoice("control", algos)
         self.control = algo(self, config)
+        if (
+            isinstance(self.control, ControlDualLoopPID)
+            and self.secondary_sensor is None
+        ):
+            raise config.error("dual_loop_pid requires a secondary sensor")
         # Setup output heater pin
         heater_pin = config.get("heater_pin")
         ppins = self.printer.lookup_object("pins")
@@ -107,7 +114,14 @@ def temperature_callback(self, read_time, temp):
             time_diff = read_time - self.last_temp_time
             self.last_temp = temp
             self.last_temp_time = read_time
-            self.control.temperature_update(read_time, temp, self.target_temp)
+            if self.secondary_sensor is not None:
+                status = self.secondary_sensor.get_status(read_time)
+                secondary_temp = status["temperature"]
+            else:
+                secondary_temp = None
+            self.control.temperature_update(
+                read_time, temp, self.target_temp, secondary_temp=secondary_temp
+            )
             temp_diff = temp - self.smoothed_temp
             adj_time = min(time_diff * self.inv_smooth_time, 1.0)
             self.smoothed_temp += temp_diff * adj_time
@@ -219,7 +233,7 @@ def __init__(self, heater, config):
         self.max_delta = config.getfloat("max_delta", 2.0, above=0.0)
         self.heating = False
 
-    def temperature_update(self, read_time, temp, target_temp):
+    def temperature_update(self, read_time, temp, target_temp, *args, **kwargs):
         if self.heating and temp >= target_temp + self.max_delta:
             self.heating = False
         elif not self.heating and temp <= target_temp - self.max_delta:
@@ -245,22 +259,29 @@ def get_type(self):
 
 
 class ControlPID:
-    def __init__(self, heater, config):
+    def __init__(
+        self,
+        heater,
+        config,
+        pid_kp_arg_name="pid_kp",
+        pid_ki_arg_name="pid_ki",
+        pid_kd_arg_name="pid_kd",
+    ):
         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._Kp = config.getfloat(pid_kp_arg_name) / PID_PARAM_BASE
+        self._Ki = config.getfloat(pid_ki_arg_name) / PID_PARAM_BASE
+        self._Kd = config.getfloat(pid_kd_arg_name) / PID_PARAM_BASE
         self.min_deriv_time = heater.get_smooth_time()
         self.temp_integ_max = 0.0
-        if self.Ki:
-            self.temp_integ_max = self.heater_max_power / self.Ki
+        if self._Ki:
+            self.temp_integ_max = self.heater_max_power / self._Ki
         self.prev_temp = AMBIENT_TEMP
         self.prev_temp_time = 0.0
         self.prev_temp_deriv = 0.0
         self.prev_temp_integ = 0.0
 
-    def temperature_update(self, read_time, temp, target_temp):
+    def calculate_output(self, read_time, temp, target_temp):
         time_diff = read_time - self.prev_temp_time
         # Calculate change of temperature
         temp_diff = temp - self.prev_temp
@@ -276,25 +297,54 @@ def temperature_update(self, read_time, temp, target_temp):
         temp_integ = self.prev_temp_integ + temp_err * time_diff
         temp_integ = max(0.0, min(self.temp_integ_max, temp_integ))
         # Calculate output
-        co = self.Kp * temp_err + self.Ki * temp_integ - self.Kd * temp_deriv
+        co = self._Kp * temp_err + self._Ki * temp_integ - self._Kd * temp_deriv
         # logging.debug("pid: %f@%.3f -> diff=%f deriv=%f err=%f integ=%f co=%d",
         #    temp, read_time, temp_diff, temp_deriv, temp_err, temp_integ, co)
         bounded_co = max(0.0, min(self.heater_max_power, co))
-        self.heater.set_pwm(read_time, bounded_co)
         # Store state for next measurement
         self.prev_temp = temp
         self.prev_temp_time = read_time
         self.prev_temp_deriv = temp_deriv
         if co == bounded_co:
             self.prev_temp_integ = temp_integ
 
+        return co, bounded_co
+
+    def temperature_update(self, read_time, temp, target_temp, *args, **kwargs):
+        _, bounded_co = self.calculate_output(read_time, temp, target_temp)
+        self.heater.set_pwm(read_time, bounded_co)
+
     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
         )
 
+    @property
+    def Kp(self):
+        return self._Kp * PID_PARAM_BASE
+
+    @property
+    def Ki(self):
+        return self._Ki * PID_PARAM_BASE
+
+    @property
+    def Kd(self):
+        return self._Kd * PID_PARAM_BASE
+
+    @Kp.setter
+    def Kp(self, value):
+        self._Kp = value / PID_PARAM_BASE
+
+    @Ki.setter
+    def Ki(self, value):
+        self._Ki = value / PID_PARAM_BASE
+
+    @Kd.setter
+    def Kd(self, value):
+        self._Kd = value / PID_PARAM_BASE
+
     def get_type(self):
         return "pid"
 
@@ -368,6 +418,51 @@ def get_type(self):
         return "pid_v"
 
 
+class ControlDualLoopPID:
+    def __init__(self, heater, config):
+        self.heater = heater
+        self.heater_max_power = heater.get_max_power()
+
+        self.primary_pid = ControlPID(
+            heater, config, "primary_pid_kp", "primary_pid_ki", "primary_pid_kd"
+        )
+        self.secondary_pid = ControlPID(
+            heater,
+            config,
+            "secondary_pid_kp",
+            "secondary_pid_ki",
+            "secondary_pid_kd",
+        )
+        arg_name = "secondary_max_temp_target"
+        self.sec_max_temp_target = config.getfloat(arg_name)
+
+    def temperature_update(
+        self,
+        read_time,
+        primary_temp,
+        target_temp,
+        secondary_temp,
+        *args,
+        **kwargs
+    ):
+
+        pco, _ = self.primary_pid.calculate_output(
+            read_time, primary_temp, target_temp
+        )
+        sco, _ = self.secondary_pid.calculate_output(
+            read_time, secondary_temp, self.sec_max_temp_target
+        )
+
+        co = min(pco, sco)
+        bounded_co = max(0.0, min(self.heater_max_power, co))
+        self.heater.set_pwm(read_time, bounded_co)
+
+    def check_busy(self, eventtime, smoothed_temp, target_temp):
+        return self.primary_pid.check_busy(
+            eventtime, smoothed_temp, target_temp
+        )
+
+
 ######################################################################
 # Sensor and heater lookup
 ######################################################################
@@ -421,10 +516,18 @@ def setup_heater(self, config, gcode_id=None):
         heater_name = config.get_name().split()[-1]
         if heater_name in self.heaters:
             raise config.error("Heater %s already registered" % (heater_name,))
-        # Setup sensor
+        # Setup 1st sensor
         sensor = self.setup_sensor(config)
+        # Setup 2nd sensor
+        secondary_sensor_name = config.get("secondary_sensor_name", None)
+        if secondary_sensor_name is not None:
+            full_name = "temperature_sensor " + secondary_sensor_name
+            secondary_sensor = self.printer.lookup_object(full_name)
+        else:
+            secondary_sensor = None
         # Create heater
-        self.heaters[heater_name] = heater = Heater(config, sensor)
+        heater = Heater(config, sensor, secondary_sensor)
+        self.heaters[heater_name] = heater
         self.register_sensor(config, heater, gcode_id)
         self.available_heaters.append(config.get_name())
         return heater