1 files changed, 82 insertions, 0 deletions
diff --git a/klippy/homing.py b/klippy/homing.py
new file mode 100644
index 000000000..566cdf95e
--- /dev/null
+++ b/klippy/homing.py
@@ -0,0 +1,82 @@
+# Code for state tracking during homing operations
+#
+# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+
+import logging
+
+class Homing:
+    def __init__(self, kin, steppers):
+        self.kin = kin
+        self.steppers = steppers
+
+        self.states = []
+        self.endstops = []
+        self.changed_axis = []
+    def plan_home(self, axis, precise=False):
+        s = self.steppers[axis]
+        state = self.states
+        self.changed_axis.append(axis)
+        speed = s.homing_speed
+        if s.homing_positive_dir:
+            pos = s.position_endstop + 1.5*(s.position_min - s.position_endstop)
+            rpos = s.position_endstop - s.homing_retract_dist
+            r2pos = rpos - s.homing_retract_dist
+        else:
+            pos = s.position_endstop + 1.5*(s.position_max - s.position_endstop)
+            rpos = s.position_endstop + s.homing_retract_dist
+            r2pos = rpos + s.homing_retract_dist
+        # Initial homing
+        state.append((self.do_home, ({axis: pos}, speed)))
+        state.append((self.do_wait_endstop, ({axis: 1},)))
+        # Retract
+        state.append((self.do_move, ({axis: rpos}, speed)))
+        # Home again
+        state.append((self.do_home, ({axis: r2pos}, speed/2.0)))
+        state.append((self.do_wait_endstop, ({axis: 1},)))
+    def plan_axis_update(self, callback):
+        self.states.append((callback, (self.changed_axis,)))
+    def check_busy(self, eventtime):
+        while self.states:
+            first = self.states[0]
+            ret = first[0](*first[1])
+            if ret:
+                return True
+            self.states.pop(0)
+        return False
+
+    def do_move(self, axis_pos, speed):
+        # Issue a move command to axis_pos
+        newpos = self.kin.get_position()
+        for axis, pos in axis_pos.items():
+            newpos[axis] = pos
+        self.kin.move(newpos, speed)
+        return False
+    def do_home(self, axis_pos, speed):
+        # Alter kinematics class to think printer is at axis_pos
+        newpos = self.kin.get_position()
+        forcepos = list(newpos)
+        for axis, pos in axis_pos.items():
+            newpos[axis] = self.steppers[axis].position_endstop
+            forcepos[axis] = pos
+        self.kin.set_position(forcepos)
+        # Start homing and issue move
+        print_time = self.kin.get_last_move_time()
+        for axis in axis_pos:
+            hz = speed * self.steppers[axis].inv_step_dist
+            es = self.steppers[axis].enable_endstop_checking(print_time, hz)
+            self.endstops.append(es)
+        self.kin.move(newpos, speed)
+        self.kin.reset_print_time()
+        for es in self.endstops:
+            es.home_finalize()
+        return False
+    def do_wait_endstop(self, axis_wait):
+        # Check if axis_wait endstops have triggered
+        for es in self.endstops:
+            if es.is_homing():
+                return True
+        # Finished
+        del self.endstops[:]
+        return False