endstop_phase: Move endstop phase homing code to new "extras" module

Move the endstop phase tracking code from stepper.py to a new
extras/endstop_phase.py module.

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

1 files changed, 75 insertions, 0 deletions

diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py
new file mode 100644
index 00000000..d3deba7c
--- /dev/null
+++ b/klippy/extras/endstop_phase.py
@@ -0,0 +1,75 @@
+# Endstop accuracy improvement via stepper phase tracking
+#
+# Copyright (C) 2016-2018  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math, logging
+import homing
+
+class EndstopPhase:
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.name = config.get_name().split()[1]
+        stepper_config = config.getsection(self.name)
+        self.step_dist = step_dist = stepper_config.getfloat('step_distance')
+        self.phases = config.getint('phases', minval=1)
+        self.endstop_phase = config.getint('endstop_phase', None,
+                                           minval=0, maxval=self.phases-1)
+        self.endstop_align_zero = config.getboolean('endstop_align_zero', False)
+        # Determine endstop accuracy
+        endstop_accuracy = config.getfloat('endstop_accuracy', None, above=0.)
+        if endstop_accuracy is None:
+            self.endstop_accuracy = self.phases//2 - 1
+        elif self.endstop_phase is not None:
+            self.endstop_accuracy = int(
+                math.ceil(endstop_accuracy * .5 / step_dist))
+        else:
+            self.endstop_accuracy = int(math.ceil(endstop_accuracy / step_dist))
+        if self.endstop_accuracy >= self.phases // 2:
+            raise config.error("Endstop for %s is not accurate enough for"
+                               " stepper phase adjustment" % (self.name,))
+        if self.printer.get_start_args().get('debugoutput') is not None:
+            self.endstop_accuracy = self.phases
+        # Register event handler
+        self.printer.register_event_handler(
+            "homing:homed_rails", self.handle_homed_rails)
+    def align_endstop(self, pos):
+        if not self.endstop_align_zero or self.endstop_phase is None:
+            return pos
+        # Adjust the endstop position so 0.0 is always at a full step
+        microsteps = self.phases // 4
+        half_microsteps = microsteps // 2
+        phase_offset = (((self.endstop_phase + half_microsteps) % microsteps)
+                        - half_microsteps) * self.step_dist
+        full_step = microsteps * self.step_dist
+        return int(pos / full_step + .5) * full_step + phase_offset
+    def get_homed_offset(self, stepper):
+        pos = stepper.get_mcu_position()
+        phase = pos % self.phases
+        if self.endstop_phase is None:
+            logging.info("Setting %s endstop phase to %d", self.name, phase)
+            self.endstop_phase = phase
+            return 0.
+        delta = (phase - self.endstop_phase) % self.phases
+        if delta >= self.phases - self.endstop_accuracy:
+            delta -= self.phases
+        elif delta > self.endstop_accuracy:
+            raise homing.EndstopError(
+                "Endstop %s incorrect phase (got %d vs %d)" % (
+                    self.name, phase, self.endstop_phase))
+        return delta * self.step_dist
+    def handle_homed_rails(self, homing_state, rails):
+        for rail in rails:
+            stepper = rail.get_steppers()[0]
+            if stepper.get_name() != self.name:
+                continue
+            orig_pos = rail.get_commanded_position()
+            offset = self.get_homed_offset(stepper)
+            pos = self.align_endstop(orig_pos) + offset
+            if pos == orig_pos:
+                return False
+            rail.set_commanded_position(pos)
+            return True
+
+def load_config_prefix(config):
+    return EndstopPhase(config)