gcode: Rework homing to use greenlets

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

6 files changed, 59 insertions, 97 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 5171297e..b2bd9661 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -28,12 +28,6 @@ class CartKinematics:
         for i in StepList:
             s = self.steppers[i]
             s.mcu_stepper.set_position(int(newpos[i]*s.inv_step_dist + 0.5))
-    def _get_homed_position(self, homing_state):
-        pos = [None]*3
-        for axis in homing_state.get_axes():
-            s = self.steppers[axis]
-            pos[axis] = s.position_endstop + s.get_homed_offset()*s.step_dist
-        return pos
     def home(self, homing_state):
         # Each axis is homed independently and in order
         for axis in homing_state.get_axes():
@@ -55,15 +49,17 @@ class CartKinematics:
             homepos[axis] = s.position_endstop
             coord = [None, None, None, None]
             coord[axis] = pos
-            homing_state.plan_home(list(coord), homepos, [s], s.homing_speed)
+            homing_state.home(list(coord), homepos, [s], s.homing_speed)
             # Retract
             coord[axis] = rpos
-            homing_state.plan_retract(list(coord), [s], s.homing_speed)
+            homing_state.retract(list(coord), s.homing_speed)
             # Home again
             coord[axis] = r2pos
-            homing_state.plan_second_home(
-                list(coord), homepos, [s], s.homing_speed/2.0)
-        homing_state.plan_calc_position(self._get_homed_position)
+            homing_state.home(
+                list(coord), homepos, [s], s.homing_speed/2.0, second_home=True)
+            # Set final homed position
+            coord[axis] = s.position_endstop + s.get_homed_offset()*s.step_dist
+            homing_state.set_homed_position(coord)
     def motor_off(self, move_time):
         self.limits = [(1.0, -1.0)] * 3
         for stepper in self.steppers:
diff --git a/klippy/delta.py b/klippy/delta.py
index bbb5f6f0..96079d1e 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -78,11 +78,6 @@ class DeltaKinematics:
         pos = self._cartesian_to_actuator(newpos)
         for i in StepList:
             self.steppers[i].mcu_stepper.set_position(pos[i])
-    def _get_homed_position(self, homing_state):
-        pos = [(s.mcu_stepper.commanded_position + s.get_homed_offset())
-               * s.step_dist
-               for s in self.steppers]
-        return self._actuator_to_cartesian(pos)
     def home(self, homing_state):
         # All axes are homed simultaneously
         homing_state.set_axes([0, 1, 2])
@@ -92,16 +87,19 @@ class DeltaKinematics:
         homepos = [0., 0., s.position_endstop, None]
         coord = list(homepos)
         coord[2] = -1.5 * math.sqrt(self.arm_length2-self.max_xy2)
-        homing_state.plan_home(list(coord), homepos, self.steppers
-                               , s.homing_speed)
+        homing_state.home(list(coord), homepos, self.steppers, s.homing_speed)
         # Retract
         coord[2] = homepos[2] - s.homing_retract_dist
-        homing_state.plan_retract(list(coord), self.steppers, s.homing_speed)
+        homing_state.retract(list(coord), s.homing_speed)
         # Home again
         coord[2] -= s.homing_retract_dist
-        homing_state.plan_second_home(list(coord), homepos, self.steppers
-                                      , s.homing_speed/2.0)
-        homing_state.plan_calc_position(self._get_homed_position)
+        homing_state.home(list(coord), homepos, self.steppers
+                          , s.homing_speed/2.0, second_home=True)
+        # Set final homed position
+        coord = [(s.mcu_stepper.commanded_position + s.get_homed_offset())
+                 * s.step_dist
+                 for s in self.steppers]
+        homing_state.set_homed_position(self._actuator_to_cartesian(coord))
     def motor_off(self, move_time):
         self.limit_xy2 = -1.
         for stepper in self.steppers:
diff --git a/klippy/gcode.py b/klippy/gcode.py
index f280b43e..5dee80cf 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -260,14 +260,16 @@ class GCodeParser:
         homing_state = homing.Homing(self.toolhead, axes)
         if self.is_fileinput:
             homing_state.set_no_verify_retract()
-        self.toolhead.home(homing_state)
-        def axes_update(homing_state):
-            newpos = self.toolhead.get_position()
-            for axis in homing_state.get_axes():
-                self.last_position[axis] = newpos[axis]
-                self.base_position[axis] = -self.homing_add[axis]
-        homing_state.plan_axes_update(axes_update)
-        self.set_busy(homing_state)
+        try:
+            self.toolhead.home(homing_state)
+        except homing.EndstopError, e:
+            self.toolhead.motor_off()
+            self.respond_error(str(e))
+            return
+        newpos = self.toolhead.get_position()
+        for axis in homing_state.get_axes():
+            self.last_position[axis] = newpos[axis]
+            self.base_position[axis] = -self.homing_add[axis]
     def cmd_G90(self, params):
         # Use absolute coordinates
         self.absolutecoord = True
diff --git a/klippy/homing.py b/klippy/homing.py
index b1126b57..1e06f0b9 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -4,91 +4,54 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging
-import mcu
 
 class Homing:
     def __init__(self, toolhead, changed_axes):
         self.toolhead = toolhead
         self.changed_axes = changed_axes
         self.verify_retract = True
-
-        self.eventtime = 0.
-        self.states = []
-        self.endstops = []
     def set_no_verify_retract(self):
         self.verify_retract = False
     def set_axes(self, axes):
         self.changed_axes = axes
     def get_axes(self):
         return self.changed_axes
-    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, (False,)))
-    def plan_second_home(self, forcepos, movepos, steppers, speed):
-        self.states.append((self.do_home, (forcepos, movepos, steppers, speed)))
-        self.states.append((self.do_wait_endstop, (self.verify_retract,)))
-    def plan_retract(self, newpos, steppers, speed):
-        self.states.append((self.do_retract, (newpos, steppers, speed)))
-    def plan_calc_position(self, callback):
-        self.states.append((self.do_calc_position, (callback,)))
-    def plan_axes_update(self, callback):
-        self.states.append((callback, (self,)))
-    def check_busy(self, eventtime):
-        self.eventtime = eventtime
-        while self.states:
-            first = self.states[0]
-            try:
-                ret = first[0](*first[1])
-            except EndstopError, e:
-                self.toolhead.motor_off()
-                raise
-            if ret:
-                return True
-            self.states.pop(0)
-        return False
-
-    def fill_coord(self, coord):
+    def _fill_coord(self, coord):
         # Fill in any None entries in 'coord' with current toolhead position
         thcoord = list(self.toolhead.get_position())
         for i in range(len(coord)):
             if coord[i] is not None:
                 thcoord[i] = coord[i]
         return thcoord
-    def do_retract(self, newpos, steppers, speed):
-        self.toolhead.move(self.fill_coord(newpos), speed)
-        return False
-    def do_home(self, forcepos, movepos, steppers, speed):
+    def retract(self, newpos, speed):
+        self.toolhead.move(self._fill_coord(newpos), speed)
+    def home(self, forcepos, movepos, steppers, speed, second_home=False):
         # Alter kinematics class to think printer is at forcepos
-        self.toolhead.set_position(self.fill_coord(forcepos))
+        self.toolhead.set_position(self._fill_coord(forcepos))
         # Start homing and issue move
         print_time = self.toolhead.get_last_move_time()
+        endstops = []
         for s in steppers:
             es = s.enable_endstop_checking(print_time, s.step_dist / speed)
-            self.endstops.append((s, es, s.mcu_stepper.get_mcu_position()))
-        self.toolhead.move(self.fill_coord(movepos), speed)
+            endstops.append((s, es, s.mcu_stepper.get_mcu_position()))
+        self.toolhead.move(self._fill_coord(movepos), speed)
         move_end_print_time = self.toolhead.get_last_move_time()
         self.toolhead.reset_print_time()
-        for s, es, last_pos in self.endstops:
+        for s, es, last_pos in endstops:
             es.home_finalize(es.print_to_mcu_time(move_end_print_time))
-        return False
-    def do_wait_endstop(self, verify_retract):
-        # Check if endstops have triggered
-        while self.endstops:
-            stepper, es, last_pos = self.endstops[0]
+        # Wait for endstops to trigger
+        for s, es, last_pos in endstops:
             try:
-                if es.check_busy(self.eventtime):
-                    return True
-            except mcu.error, e:
+                es.home_wait()
+            except es.error, e:
                 raise EndstopError("Failed to home stepper %s: %s" % (
-                    stepper.name, str(e)))
-            post_home_pos = stepper.mcu_stepper.get_mcu_position()
-            if verify_retract and last_pos == post_home_pos:
+                    s.name, str(e)))
+            post_home_pos = s.mcu_stepper.get_mcu_position()
+            if second_home and self.verify_retract and last_pos == post_home_pos:
                 raise EndstopError("Endstop %s still triggered after retract" % (
-                    stepper.name,))
-            self.endstops.pop(0)
-        return False
-    def do_calc_position(self, callback):
-        self.toolhead.set_position(self.fill_coord(callback(self)))
+                    s.name,))
+    def set_homed_position(self, pos):
+        self.toolhead.set_position(self._fill_coord(pos))
 
 class EndstopError(Exception):
     pass
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 091222b2..ba441474 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -111,6 +111,7 @@ class MCU_stepper:
         return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
 
 class MCU_endstop:
+    error = error
     RETRY_QUERY = 1.000
     def __init__(self, mcu, pin, stepper):
         self._mcu = mcu
@@ -134,7 +135,7 @@ class MCU_endstop:
         self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
         self._last_state = {}
         self.print_to_mcu_time = mcu.print_to_mcu_time
-    def home(self, mcu_time, rest_time):
+    def home_start(self, mcu_time, rest_time):
         clock = int(mcu_time * self._mcu_freq)
         rest_ticks = int(rest_time * self._mcu_freq)
         self._homing = True
@@ -149,10 +150,14 @@ class MCU_endstop:
         self._mcu.serial.send_flush()
         self._stepper.note_stepper_stop()
         self._home_timeout_clock = int(mcu_time * self._mcu_freq)
+    def home_wait(self):
+        eventtime = time.time()
+        while self._check_busy(eventtime):
+            eventtime = self._mcu.pause(eventtime + 0.1)
     def _handle_end_stop_state(self, params):
         logging.debug("end_stop_state %s" % (params,))
         self._last_state = params
-    def check_busy(self, eventtime):
+    def _check_busy(self, eventtime):
         # Check if need to send an end_stop_query command
         if self._mcu.is_fileoutput():
             return False
@@ -161,7 +166,10 @@ class MCU_endstop:
             if not self._homing:
                 return False
             if not self._last_state.get('homing', 0):
-                self._stepper.set_mcu_position(self.get_last_position())
+                pos = self._last_state.get('pos', 0)
+                if self._stepper.get_invert_dir():
+                    pos = -pos
+                self._stepper.set_mcu_position(pos)
                 self._homing = False
                 return False
             if (self._mcu.serial.get_clock(last_sent_time)
@@ -178,11 +186,6 @@ class MCU_endstop:
             msg = self._query_cmd.encode(self._oid)
             self._mcu.send(msg, cq=self._cmd_queue)
         return True
-    def get_last_position(self):
-        pos = self._last_state.get('pos', 0)
-        if self._stepper.get_invert_dir():
-            return -pos
-        return pos
     def query_endstop(self, mcu_time):
         clock = int(mcu_time * self._mcu_freq)
         self._homing = False
@@ -190,7 +193,7 @@ class MCU_endstop:
         self._next_query_clock = clock
     def query_endstop_wait(self):
         eventtime = time.time()
-        while self.check_busy(eventtime):
+        while self._check_busy(eventtime):
             eventtime = self._mcu.pause(eventtime + 0.1)
         return self._last_state.get('pin', self._invert) ^ self._invert
 
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 038412ff..1c40e6c7 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -75,7 +75,7 @@ class PrinterStepper:
         self.need_motor_enable = not enable
     def enable_endstop_checking(self, move_time, step_time):
         mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
-        self.mcu_endstop.home(mcu_time, step_time)
+        self.mcu_endstop.home_start(mcu_time, step_time)
         return self.mcu_endstop
     def query_endstop(self, print_time):
         mcu_time = self.mcu_endstop.print_to_mcu_time(print_time)
@@ -84,7 +84,7 @@ class PrinterStepper:
     def get_homed_offset(self):
         if not self.homing_stepper_phases or self.need_motor_enable:
             return 0
-        pos = self.mcu_endstop.get_last_position()
+        pos = self.mcu_stepper.get_mcu_position()
         pos %= self.homing_stepper_phases
         if self.homing_endstop_phase is None:
             logging.info("Setting %s endstop phase to %d" % (