stepper: Add get/set_tag_position() and convert calc_position()

Rename calc_position() to calc_tag_position() and have it calculate
the value of the position from the last stepper set_tag_position()
call.  This enables the calc_tag_position() code to be more flexible
as it can be run with arbitrary positions.

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

11 files changed, 56 insertions, 40 deletions

diff --git a/docs/Code_Overview.md b/docs/Code_Overview.md
index 22bbf2d3..7f4be890 100644
--- a/docs/Code_Overview.md
+++ b/docs/Code_Overview.md
@@ -330,11 +330,12 @@ Useful steps:
    seconds) to a cartesian coordinate (in millimeters), and then
    calculate the desired stepper position (in millimeters) from that
    cartesian coordinate.
-4. Implement the `calc_position()` method in the new kinematics class.
-   This method calculates the position of the toolhead in cartesian
-   coordinates from the current position of each stepper. It does not
-   need to be efficient as it is typically only called during homing
-   and probing operations.
+4. Implement the `calc_tag_position()` method in the new kinematics
+   class. This method calculates the position of the toolhead in
+   cartesian coordinates from the position of each stepper (as
+   returned by `stepper.get_tag_position()`). It does not need to be
+   efficient as it is typically only called during homing and probing
+   operations.
 5. Other methods. Implement the `check_move()`, `get_status()`,
    `get_steppers()`, `home()`, and `set_position()` methods. These
    functions are typically used to provide kinematic specific
diff --git a/klippy/extras/manual_probe.py b/klippy/extras/manual_probe.py
index 38160fef..c3fcadb9 100644
--- a/klippy/extras/manual_probe.py
+++ b/klippy/extras/manual_probe.py
@@ -83,7 +83,10 @@ class ManualProbeHelper:
         if toolhead_pos == self.last_toolhead_pos:
             return self.last_kinematics_pos
         self.toolhead.get_last_move_time()
-        kin_pos = self.toolhead.get_kinematics().calc_position()
+        kin = self.toolhead.get_kinematics()
+        for s in kin.get_steppers():
+            s.set_tag_position(s.get_commanded_position())
+        kin_pos = kin.calc_tag_position()
         self.last_toolhead_pos = toolhead_pos
         self.last_kinematics_pos = kin_pos
         return kin_pos
diff --git a/klippy/gcode.py b/klippy/gcode.py
index ff61df4e..323d5527 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -690,11 +690,12 @@ class GCodeParser:
         steppers = kin.get_steppers()
         mcu_pos = " ".join(["%s:%d" % (s.get_name(), s.get_mcu_position())
                             for s in steppers])
-        stepper_pos = " ".join(
-            ["%s:%.6f" % (s.get_name(), s.get_commanded_position())
-             for s in steppers])
-        kinematic_pos = " ".join(["%s:%.6f"  % (a, v)
-                                  for a, v in zip("XYZE", kin.calc_position())])
+        for s in steppers:
+            s.set_tag_position(s.get_commanded_position())
+        stepper_pos = " ".join(["%s:%.6f" % (s.get_name(), s.get_tag_position())
+                                for s in steppers])
+        kin_pos = " ".join(["%s:%.6f" % (a, v)
+                            for a, v in zip("XYZ", kin.calc_tag_position())])
         toolhead_pos = " ".join(["%s:%.6f" % (a, v) for a, v in zip(
             "XYZE", self.toolhead.get_position())])
         gcode_pos = " ".join(["%s:%.6f"  % (a, v)
@@ -703,16 +704,15 @@ class GCodeParser:
                              for a, v in zip("XYZE", self.base_position)])
         homing_pos = " ".join(["%s:%.6f"  % (a, v)
                                for a, v in zip("XYZ", self.homing_position)])
-        self.respond_info(
-            "mcu: %s\n"
-            "stepper: %s\n"
-            "kinematic: %s\n"
-            "toolhead: %s\n"
-            "gcode: %s\n"
-            "gcode base: %s\n"
-            "gcode homing: %s" % (
-                mcu_pos, stepper_pos, kinematic_pos, toolhead_pos,
-                gcode_pos, base_pos, homing_pos))
+        self.respond_info("mcu: %s\n"
+                          "stepper: %s\n"
+                          "kinematic: %s\n"
+                          "toolhead: %s\n"
+                          "gcode: %s\n"
+                          "gcode base: %s\n"
+                          "gcode homing: %s"
+                          % (mcu_pos, stepper_pos, kin_pos, toolhead_pos,
+                             gcode_pos, base_pos, homing_pos))
     def request_restart(self, result):
         if self.is_printer_ready:
             print_time = self.toolhead.get_last_move_time()
diff --git a/klippy/homing.py b/klippy/homing.py
index 63530a91..0f3ee308 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -67,8 +67,10 @@ class Homing:
                 if error is None:
                     error = "Failed to home %s: %s" % (name, str(e))
         if probe_pos:
-            self.set_homed_position(
-                list(self.toolhead.get_kinematics().calc_position()) + [None])
+            kin = self.toolhead.get_kinematics()
+            for s in kin.get_steppers():
+                s.set_tag_position(s.get_commanded_position())
+            self.set_homed_position(kin.calc_tag_position())
         else:
             self.toolhead.set_position(movepos)
         for mcu_endstop, name in endstops:
@@ -116,7 +118,10 @@ class Homing:
         ret = self.printer.send_event("homing:homed_rails", self, rails)
         if any(ret):
             # Apply any homing offsets
-            adjustpos = self.toolhead.get_kinematics().calc_position()
+            kin = self.toolhead.get_kinematics()
+            for s in kin.get_steppers():
+                s.set_tag_position(s.get_commanded_position())
+            adjustpos = kin.calc_tag_position()
             for axis in homing_axes:
                 movepos[axis] = adjustpos[axis]
             self.toolhead.set_position(movepos)
diff --git a/klippy/kinematics/cartesian.py b/klippy/kinematics/cartesian.py
index f8c50bb9..671914d5 100644
--- a/klippy/kinematics/cartesian.py
+++ b/klippy/kinematics/cartesian.py
@@ -53,8 +53,8 @@ class CartKinematics:
         if flags == "Z":
             return self.rails[2].get_steppers()
         return [s for rail in self.rails for s in rail.get_steppers()]
-    def calc_position(self):
-        return [rail.get_commanded_position() for rail in self.rails]
+    def calc_tag_position(self):
+        return [rail.get_tag_position() for rail in self.rails]
     def set_position(self, newpos, homing_axes):
         for i, rail in enumerate(self.rails):
             rail.set_position(newpos)
@@ -125,8 +125,9 @@ class CartKinematics:
         self.rails[dc_axis].set_trapq(None)
         dc_rail.set_trapq(toolhead.get_trapq())
         self.rails[dc_axis] = dc_rail
-        extruder_pos = toolhead.get_position()[3]
-        toolhead.set_position(self.calc_position() + [extruder_pos])
+        pos = toolhead.get_position()
+        pos[dc_axis] = dc_rail.get_commanded_position()
+        toolhead.set_position(pos)
         if self.limits[dc_axis][0] <= self.limits[dc_axis][1]:
             self.limits[dc_axis] = dc_rail.get_range()
     cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"
diff --git a/klippy/kinematics/corexy.py b/klippy/kinematics/corexy.py
index 21901720..aa88406d 100644
--- a/klippy/kinematics/corexy.py
+++ b/klippy/kinematics/corexy.py
@@ -43,8 +43,8 @@ class CoreXYKinematics:
         if flags == "Z":
             return self.rails[2].get_steppers()
         return [s for rail in self.rails for s in rail.get_steppers()]
-    def calc_position(self):
-        pos = [rail.get_commanded_position() for rail in self.rails]
+    def calc_tag_position(self):
+        pos = [rail.get_tag_position() for rail in self.rails]
         return [0.5 * (pos[0] + pos[1]), 0.5 * (pos[0] - pos[1]), pos[2]]
     def set_position(self, newpos, homing_axes):
         for i, rail in enumerate(self.rails):
diff --git a/klippy/kinematics/delta.py b/klippy/kinematics/delta.py
index af3aa092..ee01be56 100644
--- a/klippy/kinematics/delta.py
+++ b/klippy/kinematics/delta.py
@@ -92,8 +92,8 @@ class DeltaKinematics:
     def _actuator_to_cartesian(self, spos):
         sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
         return mathutil.trilateration(sphere_coords, self.arm2)
-    def calc_position(self):
-        spos = [rail.get_commanded_position() for rail in self.rails]
+    def calc_tag_position(self):
+        spos = [rail.get_tag_position() for rail in self.rails]
         return self._actuator_to_cartesian(spos)
     def set_position(self, newpos, homing_axes):
         for rail in self.rails:
diff --git a/klippy/kinematics/none.py b/klippy/kinematics/none.py
index 2250f884..6fe9884f 100644
--- a/klippy/kinematics/none.py
+++ b/klippy/kinematics/none.py
@@ -9,7 +9,7 @@ class NoneKinematics:
         pass
     def get_steppers(self, flags=""):
         return []
-    def calc_position(self):
+    def calc_tag_position(self):
         return [0, 0, 0]
     def set_position(self, newpos, homing_axes):
         pass
diff --git a/klippy/kinematics/polar.py b/klippy/kinematics/polar.py
index ca2e55c7..3a043ab3 100644
--- a/klippy/kinematics/polar.py
+++ b/klippy/kinematics/polar.py
@@ -40,10 +40,10 @@ class PolarKinematics:
         if flags == "Z":
             return self.rails[1].get_steppers()
         return list(self.steppers)
-    def calc_position(self):
-        bed_angle = self.steppers[0].get_commanded_position()
-        arm_pos = self.rails[0].get_commanded_position()
-        z_pos = self.rails[1].get_commanded_position()
+    def calc_tag_position(self):
+        bed_angle = self.steppers[0].get_tag_position()
+        arm_pos = self.rails[0].get_tag_position()
+        z_pos = self.rails[1].get_tag_position()
         return [math.cos(bed_angle) * arm_pos, math.sin(bed_angle) * arm_pos,
                 z_pos]
     def set_position(self, newpos, homing_axes):
diff --git a/klippy/kinematics/winch.py b/klippy/kinematics/winch.py
index 32d388ad..6792dbd7 100644
--- a/klippy/kinematics/winch.py
+++ b/klippy/kinematics/winch.py
@@ -31,9 +31,9 @@ class WinchKinematics:
         self.set_position([0., 0., 0.], ())
     def get_steppers(self, flags=""):
         return list(self.steppers)
-    def calc_position(self):
+    def calc_tag_position(self):
         # Use only first three steppers to calculate cartesian position
-        spos = [s.get_commanded_position() for s in self.steppers[:3]]
+        spos = [s.get_tag_position() for s in self.steppers[:3]]
         return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in spos])
     def set_position(self, newpos, homing_axes):
         for s in self.steppers:
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 97a1283e..eff0fa67 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -29,7 +29,7 @@ class MCU_stepper:
                 "Stepper dir pin must be on same mcu as step pin")
         self._dir_pin = dir_pin_params['pin']
         self._invert_dir = dir_pin_params['invert']
-        self._mcu_position_offset = 0.
+        self._mcu_position_offset = self._tag_position = 0.
         self._min_stop_interval = 0.
         self._reset_cmd_id = self._get_position_cmd = None
         self._active_callbacks = []
@@ -107,6 +107,10 @@ class MCU_stepper:
         if mcu_pos >= 0.:
             return int(mcu_pos + 0.5)
         return int(mcu_pos - 0.5)
+    def get_tag_position(self):
+        return self._tag_position
+    def set_tag_position(self, position):
+        self._tag_position = position
     def set_stepper_kinematics(self, sk):
         old_sk = self._stepper_kinematics
         self._stepper_kinematics = sk
@@ -193,6 +197,8 @@ class PrinterRail:
         self.endstops = []
         self.add_extra_stepper(config)
         self.get_commanded_position = self.steppers[0].get_commanded_position
+        self.get_tag_position = self.steppers[0].get_tag_position
+        self.set_tag_position = self.steppers[0].set_tag_position
         # Primary endstop position
         mcu_endstop = self.endstops[0][0]
         if hasattr(mcu_endstop, "get_position_endstop"):