diff --git a/klippy/extras/heater_bed.py b/klippy/extras/heater_bed.py
index c7f69e50..96afb8fe 100644
--- a/klippy/extras/heater_bed.py
+++ b/klippy/extras/heater_bed.py
@@ -18,10 +18,8 @@ class PrinterHeaterBed:
     def cmd_M140(self, gcmd, wait=False):
         # Set Bed Temperature
         temp = gcmd.get_float('S', 0.)
-        self.heater.set_temp(temp)
-        if wait and temp:
-            pheaters = self.printer.lookup_object('heaters')
-            pheaters.wait_for_temperature(self.heater)
+        pheaters = self.printer.lookup_object('heaters')
+        pheaters.set_temperature(self.heater, temp, wait)
     def cmd_M190(self, gcmd):
         # Set Bed Temperature and Wait
         self.cmd_M140(gcmd, wait=True)
diff --git a/klippy/extras/heaters.py b/klippy/extras/heaters.py
index cf7fd909..e63881dc 100644
--- a/klippy/extras/heaters.py
+++ b/klippy/extras/heaters.py
@@ -138,7 +138,8 @@ class Heater:
     cmd_SET_HEATER_TEMPERATURE_help = "Sets a heater temperature"
     def cmd_SET_HEATER_TEMPERATURE(self, gcmd):
         temp = gcmd.get_float('TARGET', 0.)
-        self.set_temp(temp)
+        pheaters = self.printer.lookup_object('heaters')
+        pheaters.set_temperature(self, temp)
 
 
 ######################################################################
@@ -313,7 +314,7 @@ class PrinterHeaters:
         did_ack = gcmd.ack(msg)
         if not did_ack:
             gcmd.respond_raw(msg)
-    def wait_for_temperature(self, heater):
+    def _wait_for_temperature(self, heater):
         # Helper to wait on heater.check_busy() and report M105 temperatures
         if self.printer.get_start_args().get('debugoutput') is not None:
             return
@@ -325,6 +326,12 @@ class PrinterHeaters:
             print_time = toolhead.get_last_move_time()
             gcode.respond_raw(self._get_temp(eventtime))
             eventtime = reactor.pause(eventtime + 1.)
+    def set_temperature(self, heater, temp, wait=False):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.register_lookahead_callback((lambda pt: None))
+        heater.set_temp(temp)
+        if wait and temp:
+            self._wait_for_temperature(heater)
     cmd_TEMPERATURE_WAIT_help = "Wait for a temperature on a sensor"
     def cmd_TEMPERATURE_WAIT(self, gcmd):
         sensor_name = gcmd.get('SENSOR')
diff --git a/klippy/extras/pid_calibrate.py b/klippy/extras/pid_calibrate.py
index a00f0a1b..f269a373 100644
--- a/klippy/extras/pid_calibrate.py
+++ b/klippy/extras/pid_calibrate.py
@@ -26,11 +26,10 @@ class PIDCalibrate:
         calibrate = ControlAutoTune(heater, target)
         old_control = heater.set_control(calibrate)
         try:
-            heater.set_temp(target)
+            pheaters.set_temperature(heater, target, True)
         except self.printer.command_error as e:
             heater.set_control(old_control)
             raise
-        pheaters.wait_for_temperature(heater)
         heater.set_control(old_control)
         if write_file:
             calibrate.write_file('/tmp/heattest.txt')
diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py
index 591a0531..e767abef 100644
--- a/klippy/kinematics/extruder.py
+++ b/klippy/kinematics/extruder.py
@@ -167,10 +167,8 @@ class PrinterExtruder:
                 raise gcmd.error("Extruder not configured")
         else:
             extruder = self.printer.lookup_object('toolhead').get_extruder()
-        heater = extruder.get_heater()
-        heater.set_temp(temp)
-        if wait and temp:
-            self.printer.lookup_object('heaters').wait_for_temperature(heater)
+        pheaters = self.printer.lookup_object('heaters')
+        pheaters.set_temperature(extruder.get_heater(), temp, wait)
     def cmd_M109(self, gcmd):
         # Set Extruder Temperature and Wait
         self.cmd_M104(gcmd, wait=True)