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>

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
+4. Implement the `calc_position()` method in the new kinematics class.
-   calculates the desired stepper positions given a cartesian
-   coordinate.
-5. 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
+5. Other methods. The `move()`, `home()`, `check_move()`, and other
-   iterative solver for each stepper.
+   methods should also be implemented. These functions are typically
-7. Other methods. The `home()`, `check_move()`, and other methods
+   used to provide kinematic specific checks. However, at the start of
-   should also be implemented. However, at the start of development
+   development one can use boiler-plate code here.
-   one can use empty code here.
+6. Implement test cases. Create a g-code file with a series of moves
-8. 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

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:
+        for i, rail in enumerate(self.rails):
-            rail = self.rails[i]
+            rail.set_position(newpos)
-            rail.set_position(newpos[i])
             if i in homing_axes:
                 self.limits[i] = rail.get_range()
     def _home_axis(self, homing_state, axis, rail):

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);
 """

klippy/chelper/itersolve.c

@@ -215,6 +215,16 @@ itersolve_set_stepcompress(struct stepper_kinematics *sk
     sk->step_dist = step_dist;
 }
 
+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)
 {

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

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, rail in enumerate(self.rails):
-        for i in StepList:
+            rail.set_position(newpos)
-            rail = self.rails[i]
-            rail.set_position(pos[i])
             if i in homing_axes:
                 self.limits[i] = rail.get_range()
     def home(self, homing_state):

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 rail in self.rails:
-        for i in StepList:
+            rail.set_position(newpos)
-            self.rails[i].set_position(pos[i])
         self.limit_xy2 = -1.
         if tuple(homing_axes) == StepList:
             self.need_home = False

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):
+    def set_position(self, newpos):
-        self._mcu_position_offset += self.get_commanded_position() - pos
+        orig_cmd_pos = self.get_commanded_position()
-        self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos)
+        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)

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)