stepper: Support stepper phase adjustments when homing

Add support for enhancing the precision of endstop switches by also
inspecting the phase of the stepper motor when the endstop triggers.

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

4 files changed, 54 insertions, 1 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index f061f673..b904d73f 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -33,6 +33,8 @@ class CartKinematics:
         accel_factor = min([self.steppers[i].max_accel / abs(axes_d[i])
                             for i in StepList if axes_d[i]])
         return velocity_factor * move_d, accel_factor * move_d
+    def get_homed_position(self):
+        return [s.get_homed_position() for s in self.steppers]
     def home(self, toolhead, axes):
         # Each axis is homed independently and in order
         homing_state = homing.Homing(toolhead, axes)
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 728ec91d..864ebbfb 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -40,6 +40,8 @@ class MCU_stepper:
             max_error, self._step_cmd.msgid, self._oid)
     def get_oid(self):
         return self._oid
+    def get_invert_dir(self):
+        return self._invert_dir
     def note_stepper_stop(self):
         self._sdir = -1
         self._last_move_clock = -2**29
@@ -88,6 +90,7 @@ class MCU_endstop:
                          , self._oid)
         self._query_cmd = mcu.lookup_command("end_stop_query oid=%c")
         self._homing = False
+        self._last_position = 0
         self._next_query_clock = 0
         mcu_freq = self._mcu.get_mcu_freq()
         self._retry_query_ticks = mcu_freq * self.RETRY_QUERY
@@ -104,6 +107,7 @@ class MCU_endstop:
         self._stepper.note_stepper_stop()
     def _handle_end_stop_state(self, params):
         logging.debug("end_stop_state %s" % (params,))
+        self._last_position = params['pos']
         self._homing = params['homing'] != 0
     def is_homing(self):
         if not self._homing:
@@ -116,6 +120,10 @@ class MCU_endstop:
             msg = self._query_cmd.encode(self._oid)
             self._mcu.send(msg, cq=self._cmd_queue)
         return self._homing
+    def get_last_position(self):
+        if self._stepper.get_invert_dir():
+            return -self._last_position
+        return self._last_position
     def get_print_clock(self, print_time):
         return self._mcu.get_print_clock(print_time)
 
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 7c8438fc..c15d194c 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -21,6 +21,23 @@ class PrinterStepper:
         self.homing_positive_dir = config.getboolean(
             'homing_positive_dir', False)
         self.homing_retract_dist = config.getfloat('homing_retract_dist', 5.)
+        self.homing_stepper_phases = config.getint('homing_stepper_phases')
+        self.homing_endstop_phase = config.getint('homing_endstop_phase')
+        endstop_accuracy = config.getfloat('homing_endstop_accuracy')
+        self.homing_endstop_accuracy = None
+        if self.homing_stepper_phases:
+            if endstop_accuracy is None:
+                self.homing_endstop_accuracy = self.homing_stepper_phases//2 - 1
+            elif self.homing_endstop_phase is not None:
+                self.homing_endstop_accuracy = int(math.ceil(
+                    endstop_accuracy * self.inv_step_dist / 2.))
+            else:
+                self.homing_endstop_accuracy = int(math.ceil(
+                    endstop_accuracy * self.inv_step_dist))
+            if self.homing_endstop_accuracy >= self.homing_stepper_phases/2:
+                logging.info("Endstop for %s is not accurate enough for stepper"
+                             " phase adjustment" % (self.config.section,))
+                self.homing_stepper_phases = None
         self.position_min = config.getfloat('position_min', 0.)
         self.position_endstop = config.getfloat('position_endstop')
         self.position_max = config.getfloat('position_max')
@@ -68,3 +85,21 @@ class PrinterStepper:
         move_clock = int(self.mcu_endstop.get_print_clock(move_time))
         self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz))
         return self.mcu_endstop
+    def get_homed_position(self):
+        if not self.homing_stepper_phases:
+            return self.position_endstop
+        pos = self.mcu_endstop.get_last_position()
+        pos %= self.homing_stepper_phases
+        if self.homing_endstop_phase is None:
+            logging.info("Setting %s endstop phase to %d" % (
+                self.config.section, pos))
+            self.homing_endstop_phase = pos
+            return self.position_endstop
+        delta = (pos - self.homing_endstop_phase) % self.homing_stepper_phases
+        if delta >= self.homing_stepper_phases - self.homing_endstop_accuracy:
+            delta -= self.homing_stepper_phases
+        elif delta > self.homing_endstop_accuracy:
+            logging.error("Endstop %s incorrect phase (got %d vs %d)" % (
+                self.config.section, pos, self.homing_endstop_phase))
+            return self.position_endstop
+        return self.position_endstop + delta * self.step_dist
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 35d296e1..8c389d19 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -254,7 +254,15 @@ class ToolHead:
         move = Move(self, newpos, move_d, axes_d, speed, self.max_xy_accel)
         self.move_queue.add_move(move)
     def home(self, axes):
-        return self.kin.home(self, axes)
+        homing = self.kin.home(self, axes)
+        def axes_update(axes):
+            pos = self.get_position()
+            homepos = self.kin.get_homed_position()
+            for axis in axes:
+                pos[axis] = homepos[axis]
+            self.set_position(pos)
+        homing.plan_axes_update(axes_update)
+        return homing
     def dwell(self, delay):
         self.get_last_move_time()
         self.update_move_time(delay)