itersolve: Support setting the stepper position via a cartesian coordinate

Add support for an itersolve_set_position() function that sets a
stepper position from a cartesian coordinate.  This eliminates the
need for both the python and C code to be able to translate from a
cartesian coordinate to a stepper position.

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

9 files changed, 34 insertions, 29 deletions

diff --git a/docs/Code_Overview.md b/docs/Code_Overview.md
index 1bb883f4..64d54c28 100644
--- a/docs/Code_Overview.md
+++ b/docs/Code_Overview.md
@@ -319,19 +319,15 @@ 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 `set_position()` method in the python code. This also
-   calculates the desired stepper positions given a cartesian
-   coordinate.
-5. Implement the `calc_position()` method in the new kinematics class.
+4. Implement the `calc_position()` method in the new kinematics class.
    This method is the inverse of set_position(). It does not need to
    be efficient as it is typically only called during homing and
    probing operations.
-6. Implement the `move()` method. This method generally invokes the
-   iterative solver for each stepper.
-7. Other methods. The `home()`, `check_move()`, and other methods
-   should also be implemented. However, at the start of development
-   one can use empty code here.
-8. Implement test cases. Create a g-code file with a series of moves
+5. Other methods. The `move()`, `home()`, `check_move()`, and other
+   methods should also be implemented. These functions are typically
+   used to provide kinematic specific checks. However, at the start of
+   development one can use boiler-plate code here.
+6. Implement test cases. Create a g-code file with a series of moves
    that can test important cases for the given kinematics. Follow the
    [debugging documentation](Debugging.md) to convert this g-code file
    to micro-controller commands. This is useful to exercise corner
diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 7fc1419e..de6eb1df 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -54,9 +54,8 @@ class CartKinematics:
     def calc_position(self):
         return [rail.get_commanded_position() for rail in self.rails]
     def set_position(self, newpos, homing_axes):
-        for i in StepList:
-            rail = self.rails[i]
-            rail.set_position(newpos[i])
+        for i, rail in enumerate(self.rails):
+            rail.set_position(newpos)
             if i in homing_axes:
                 self.limits[i] = rail.get_range()
     def _home_axis(self, homing_state, axis, rail):
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 027cf277..083fe91b 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -51,6 +51,8 @@ defs_itersolve = """
     int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
     void itersolve_set_stepcompress(struct stepper_kinematics *sk
         , struct stepcompress *sc, double step_dist);
+    void itersolve_set_position(struct stepper_kinematics *sk
+        , double x, double y, double z);
     void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
     double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
 """
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index c9ce9b8f..2bd27ac6 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -216,6 +216,16 @@ itersolve_set_stepcompress(struct stepper_kinematics *sk
 }
 
 void __visible
+itersolve_set_position(struct stepper_kinematics *sk
+                       , double x, double y, double z)
+{
+    struct move m;
+    memset(&m, 0, sizeof(m));
+    move_fill(&m, 0., 0., 1., 0., x, y, z, 0., 1., 0., 0., 1., 0.);
+    sk->commanded_pos = sk->calc_position(sk, &m, 0.);
+}
+
+void __visible
 itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos)
 {
     sk->commanded_pos = pos;
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index 23b3201d..25f26451 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -41,6 +41,9 @@ struct stepper_kinematics {
 int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
 void itersolve_set_stepcompress(struct stepper_kinematics *sk
                                 , struct stepcompress *sc, double step_dist);
+void itersolve_set_position(struct stepper_kinematics *sk
+                            , double x, double y, double z);
 void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
+double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
 
 #endif // itersolve.h
diff --git a/klippy/corexy.py b/klippy/corexy.py
index 773b46d6..511e8fe3 100644
--- a/klippy/corexy.py
+++ b/klippy/corexy.py
@@ -46,10 +46,8 @@ class CoreXYKinematics:
         pos = [rail.get_commanded_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):
-        pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2])
-        for i in StepList:
-            rail = self.rails[i]
-            rail.set_position(pos[i])
+        for i, rail in enumerate(self.rails):
+            rail.set_position(newpos)
             if i in homing_axes:
                 self.limits[i] = rail.get_range()
     def home(self, homing_state):
diff --git a/klippy/delta.py b/klippy/delta.py
index efcfe7a5..d80959b5 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -87,19 +87,14 @@ class DeltaKinematics:
         self.set_position([0., 0., 0.], ())
     def get_rails(self, flags=""):
         return list(self.rails)
-    def _cartesian_to_actuator(self, coord):
-        return [math.sqrt(self.arm2[i] - (self.towers[i][0] - coord[0])**2
-                          - (self.towers[i][1] - coord[1])**2) + coord[2]
-                for i in StepList]
     def _actuator_to_cartesian(self, pos):
         return actuator_to_cartesian(self.towers, self.arm2, pos)
     def calc_position(self):
         spos = [rail.get_commanded_position() for rail in self.rails]
         return self._actuator_to_cartesian(spos)
     def set_position(self, newpos, homing_axes):
-        pos = self._cartesian_to_actuator(newpos)
-        for i in StepList:
-            self.rails[i].set_position(pos[i])
+        for rail in self.rails:
+            rail.set_position(newpos)
         self.limit_xy2 = -1.
         if tuple(homing_axes) == StepList:
             self.need_home = False
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 1969ff82..8d7e86cb 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -71,9 +71,11 @@ class MCU_stepper:
         return self._oid
     def get_step_dist(self):
         return self._step_dist
-    def set_position(self, pos):
-        self._mcu_position_offset += self.get_commanded_position() - pos
-        self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos)
+    def set_position(self, newpos):
+        orig_cmd_pos = self.get_commanded_position()
+        self._ffi_lib.itersolve_set_position(
+            self._stepper_kinematics, newpos[0], newpos[1], newpos[2])
+        self._mcu_position_offset += orig_cmd_pos - self.get_commanded_position()
     def get_commanded_position(self):
         return self._ffi_lib.itersolve_get_commanded_pos(
             self._stepper_kinematics)
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 5a9bee70..e68512f7 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -243,9 +243,9 @@ class PrinterRail:
     def set_max_jerk(self, max_halt_velocity, max_accel):
         for stepper in self.steppers:
             stepper.set_max_jerk(max_halt_velocity, max_accel)
-    def set_position(self, pos):
+    def set_position(self, newpos):
         for stepper in self.steppers:
-            stepper.set_position(pos)
+            stepper.set_position(newpos)
     def motor_enable(self, print_time, enable=0):
         for stepper in self.steppers:
             stepper.motor_enable(print_time, enable)