homing: Direct stepper phase detection from kinematic classes

Change the scheduling of the final homed position (which takes into
account the stepper phases) so that it is scheduled from the kinematic
classes instead of from the toolhead class.

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

4 files changed, 17 insertions, 16 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index c1cddc7d..706ebc5e 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -28,9 +28,12 @@ class CartKinematics:
         max_xy_speed = min(s.max_velocity for s in self.steppers[:2])
         max_xy_accel = min(s.max_accel for s in self.steppers[:2])
         return max_xy_speed, max_xy_accel
-    def get_homed_position(self):
-        return [s.position_endstop + s.get_homed_offset()*s.step_dist
-                for s in self.steppers]
+    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():
@@ -59,6 +62,7 @@ class CartKinematics:
             # Home again
             coord[axis] = r2pos
             homing_state.plan_home(list(coord), homepos, [s], s.homing_speed/2.0)
+        homing_state.plan_calc_position(self.get_homed_position)
     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 34b5a9e2..d54c0a3c 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -78,7 +78,7 @@ class DeltaKinematics:
         return matrix_sub(circumcenter, matrix_mul(normal, dist))
     def set_position(self, newpos):
         self.stepper_pos = self.cartesian_to_actuator(newpos)
-    def get_homed_position(self):
+    def get_homed_position(self, homing_state):
         pos = [(self.stepper_pos[i] + self.steppers[i].get_homed_offset())
                * self.steppers[i].step_dist
                for i in StepList]
@@ -101,6 +101,7 @@ class DeltaKinematics:
         coord[2] -= s.homing_retract_dist
         homing_state.plan_home(list(coord), homepos, self.steppers
                                , s.homing_speed/2.0)
+        homing_state.plan_calc_position(self.get_homed_position)
     def motor_off(self, move_time):
         self.limit_xy2 = -1.
         for stepper in self.steppers:
diff --git a/klippy/homing.py b/klippy/homing.py
index 29c1a48f..b4c5290f 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -23,6 +23,8 @@ class Homing:
         self.states.append((self.do_wait_endstop, ()))
     def plan_move(self, newpos, speed):
         self.states.append((self.do_move, (newpos, 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):
@@ -37,12 +39,11 @@ class Homing:
 
     def fill_coord(self, coord):
         # Fill in any None entries in 'coord' with current toolhead position
-        thcoord = self.toolhead.get_position()
-        coord = list(coord)
+        thcoord = list(self.toolhead.get_position())
         for i in range(len(coord)):
-            if coord[i] is None:
-                coord[i] = thcoord[i]
-        return coord
+            if coord[i] is not None:
+                thcoord[i] = coord[i]
+        return thcoord
     def do_move(self, newpos, speed):
         # Issue a move command
         self.toolhead.move(self.fill_coord(newpos), speed)
@@ -68,6 +69,8 @@ class Homing:
         # Finished
         del self.endstops[:]
         return False
+    def do_calc_position(self, callback):
+        self.toolhead.set_position(self.fill_coord(callback(self)))
 
 # Helper code for querying and reporting the status of the endstops
 class QueryEndstops:
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index bc0356dc..5248a589 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -261,13 +261,6 @@ class ToolHead:
         self.move_queue.add_move(move)
     def home(self, homing_state):
         self.kin.home(homing_state)
-        def axes_update(homing_state):
-            pos = self.get_position()
-            homepos = self.kin.get_homed_position()
-            for axis in homing_state.get_axes():
-                pos[axis] = homepos[axis]
-            self.set_position(pos)
-        homing_state.plan_axes_update(axes_update)
     def dwell(self, delay):
         self.get_last_move_time()
         self.update_move_time(delay)