corexy: Convert corexy to use the iterative solver

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

1 files changed, 48 insertions, 0 deletions

diff --git a/klippy/chelper/kin_cartesian.c b/klippy/chelper/kin_cartesian.c
index 5cefb119..c9d01e0a 100644
--- a/klippy/chelper/kin_cartesian.c
+++ b/klippy/chelper/kin_cartesian.c
@@ -5,10 +5,18 @@
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include <math.h> // sqrt
+#include <stdlib.h> // malloc
+#include <string.h> // memset
 #include "compiler.h" // likely
+#include "itersolve.h" // move_get_coord
 #include "pyhelper.h" // errorf
 #include "stepcompress.h" // queue_append
 
+
+/****************************************************************
+ * Direct step generation
+ ****************************************************************/
+
 // Common suffixes: _sd is step distance (a unit length the same
 // distance the stepper moves on each step), _sv is step velocity (in
 // units of step distance per time), _sd2 is step distance squared, _r
@@ -111,3 +119,43 @@ stepcompress_push_const(
     }
     return res;
 }
+
+
+/****************************************************************
+ * Iterative solver
+ ****************************************************************/
+
+static double
+cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).x;
+}
+
+static double
+cart_stepper_y_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).y;
+}
+
+static double
+cart_stepper_z_calc_position(struct stepper_kinematics *sk, struct move *m
+                             , double move_time)
+{
+    return move_get_coord(m, move_time).z;
+}
+
+struct stepper_kinematics * __visible
+cartesian_stepper_alloc(char axis)
+{
+    struct stepper_kinematics *sk = malloc(sizeof(*sk));
+    memset(sk, 0, sizeof(*sk));
+    if (axis == 'x')
+        sk->calc_position = cart_stepper_x_calc_position;
+    else if (axis == 'y')
+        sk->calc_position = cart_stepper_y_calc_position;
+    else if (axis == 'z')
+        sk->calc_position = cart_stepper_z_calc_position;
+    return sk;
+}