homing: Verify the endstops are no longer triggered after retract move

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

docs/Todo.md

@@ -86,8 +86,6 @@ Safety features
   can be useful to detect a sensor failure (eg, thermistor short) that
   could otherwise cause the PID to command excessive heating.
 
-* Verify the endstop is no longer triggered after retraction.
-
 * Possibly implement host based checking on the ratio between extrude
   amount and head movement.
  * Enforce acceleration and speed limits on extruder stepper motor.

klippy/cartesian.py

@@ -58,7 +58,7 @@ class CartKinematics:
             homing_state.plan_home(list(coord), homepos, [s], s.homing_speed)
             # Retract
             coord[axis] = rpos
-            homing_state.plan_move(list(coord), s.homing_speed)
+            homing_state.plan_retract(list(coord), [s], s.homing_speed)
             # Home again
             coord[axis] = r2pos
             homing_state.plan_home(list(coord), homepos, [s], s.homing_speed/2.0)

klippy/delta.py

@@ -96,7 +96,7 @@ class DeltaKinematics:
                                , s.homing_speed)
         # Retract
         coord[2] = homepos[2] - s.homing_retract_dist
-        homing_state.plan_move(list(coord), s.homing_speed)
+        homing_state.plan_retract(list(coord), self.steppers, s.homing_speed)
         # Home again
         coord[2] -= s.homing_retract_dist
         homing_state.plan_home(list(coord), homepos, self.steppers

klippy/homing.py

@@ -21,8 +21,9 @@ class Homing:
     def plan_home(self, forcepos, movepos, steppers, speed):
         self.states.append((self.do_home, (forcepos, movepos, steppers, speed)))
         self.states.append((self.do_wait_endstop, ()))
-    def plan_move(self, newpos, speed):
+    def plan_retract(self, newpos, steppers, speed):
-        self.states.append((self.do_move, (newpos, speed)))
+        self.states.append((self.do_retract, (newpos, steppers, speed)))
+        self.states.append((self.do_verify_retract, ()))
     def plan_calc_position(self, callback):
         self.states.append((self.do_calc_position, (callback,)))
     def plan_axes_update(self, callback):
@@ -48,9 +49,14 @@ class Homing:
             if coord[i] is not None:
                 thcoord[i] = coord[i]
         return thcoord
-    def do_move(self, newpos, speed):
+    def do_retract(self, newpos, steppers, speed):
         # Issue a move command
         self.toolhead.move(self.fill_coord(newpos), speed)
+        # Query endstops
+        print_time = self.toolhead.get_last_move_time()
+        for s in steppers:
+            es = s.query_endstop(print_time)
+            self.endstops.append((s.name, es))
         return False
     def do_home(self, forcepos, movepos, steppers, speed):
         # Alter kinematics class to think printer is at forcepos
@@ -68,15 +74,29 @@ class Homing:
         return False
     def do_wait_endstop(self):
         # Check if endstops have triggered
-        for name, es in self.endstops:
+        while self.endstops:
+            name, es = self.endstops[0]
             try:
                 if es.check_busy(self.eventtime):
                     return True
             except mcu.MCUError, e:
                 raise EndstopError("Failed to home stepper %s: %s" % (
                     name, str(e)))
-        # Finished
+            self.endstops.pop(0)
-        del self.endstops[:]
+        return False
+    def do_verify_retract(self):
+        while self.endstops:
+            name, es = self.endstops[0]
+            try:
+                if es.check_busy(self.eventtime):
+                    return True
+            except mcu.MCUError, e:
+                raise EndstopError("Failed to retract stepper %s: %s" % (
+                    name, str(e)))
+            if es.get_last_triggered():
+                raise EndstopError("Endstop %s still triggered after retract" % (
+                    name,))
+            self.endstops.pop(0)
         return False
     def do_calc_position(self, callback):
         self.toolhead.set_position(self.fill_coord(callback(self)))