idex_modes: COPY and MIRROR mode implementation (#6297)

COPY and MIRROR mode implementation

Correctly apply input shaper params to new dual_carriage

Added SAVE_/RESTORE_IDEX_STATE commands

Documentation updates for the new IDEX modes

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>

4 files changed, 95 insertions, 46 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 8183fd62..c94e9677 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -21,7 +21,7 @@ SOURCE_FILES = [
     'pollreactor.c', 'msgblock.c', 'trdispatch.c',
     'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
     'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c',
-    'kin_extruder.c', 'kin_shaper.c',
+    'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c',
 ]
 DEST_LIB = "c_helper.so"
 OTHER_FILES = [
@@ -101,7 +101,6 @@ defs_trapq = """
 
 defs_kin_cartesian = """
     struct stepper_kinematics *cartesian_stepper_alloc(char axis);
-    struct stepper_kinematics *cartesian_reverse_stepper_alloc(char axis);
 """
 
 defs_kin_corexy = """
@@ -153,6 +152,14 @@ defs_kin_shaper = """
     struct stepper_kinematics * input_shaper_alloc(void);
 """
 
+defs_kin_idex = """
+    void dual_carriage_set_sk(struct stepper_kinematics *sk
+        , struct stepper_kinematics *orig_sk);
+    int dual_carriage_set_transform(struct stepper_kinematics *sk
+        , char axis, double scale, double offs);
+    struct stepper_kinematics * dual_carriage_alloc(void);
+"""
+
 defs_serialqueue = """
     #define MESSAGE_MAX 64
     struct pull_queue_message {
@@ -211,7 +218,7 @@ defs_all = [
     defs_itersolve, defs_trapq, defs_trdispatch,
     defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta,
     defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch,
-    defs_kin_extruder, defs_kin_shaper,
+    defs_kin_extruder, defs_kin_shaper, defs_kin_idex,
 ]
 
 # Update filenames to an absolute path
diff --git a/klippy/chelper/kin_cartesian.c b/klippy/chelper/kin_cartesian.c
index 3b1c8cba..86569d4a 100644
--- a/klippy/chelper/kin_cartesian.c
+++ b/klippy/chelper/kin_cartesian.c
@@ -49,42 +49,3 @@ cartesian_stepper_alloc(char axis)
     }
     return sk;
 }
-
-static double
-cart_reverse_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_reverse_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_reverse_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_reverse_stepper_alloc(char axis)
-{
-    struct stepper_kinematics *sk = malloc(sizeof(*sk));
-    memset(sk, 0, sizeof(*sk));
-    if (axis == 'x') {
-        sk->calc_position_cb = cart_reverse_stepper_x_calc_position;
-        sk->active_flags = AF_X;
-    } else if (axis == 'y') {
-        sk->calc_position_cb = cart_reverse_stepper_y_calc_position;
-        sk->active_flags = AF_Y;
-    } else if (axis == 'z') {
-        sk->calc_position_cb = cart_reverse_stepper_z_calc_position;
-        sk->active_flags = AF_Z;
-    }
-    return sk;
-}
diff --git a/klippy/chelper/kin_idex.c b/klippy/chelper/kin_idex.c
new file mode 100644
index 00000000..7657a73d
--- /dev/null
+++ b/klippy/chelper/kin_idex.c
@@ -0,0 +1,81 @@
+// Idex dual carriage kinematics
+//
+// Copyright (C) 2023  Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // memset
+#include "compiler.h" // __visible
+#include "itersolve.h" // struct stepper_kinematics
+#include "trapq.h" // struct move
+
+#define DUMMY_T 500.0
+
+struct dual_carriage_stepper {
+    struct stepper_kinematics sk;
+    struct stepper_kinematics *orig_sk;
+    struct move m;
+    double x_scale, x_offs, y_scale, y_offs;
+};
+
+double
+dual_carriage_calc_position(struct stepper_kinematics *sk, struct move *m
+                            , double move_time)
+{
+    struct dual_carriage_stepper *dc = container_of(
+            sk, struct dual_carriage_stepper, sk);
+    struct coord pos = move_get_coord(m, move_time);
+    dc->m.start_pos.x = pos.x * dc->x_scale + dc->x_offs;
+    dc->m.start_pos.y = pos.y * dc->y_scale + dc->y_offs;
+    dc->m.start_pos.z = pos.z;
+    return dc->orig_sk->calc_position_cb(dc->orig_sk, &dc->m, DUMMY_T);
+}
+
+void __visible
+dual_carriage_set_sk(struct stepper_kinematics *sk
+                     , struct stepper_kinematics *orig_sk)
+{
+    struct dual_carriage_stepper *dc = container_of(
+            sk, struct dual_carriage_stepper, sk);
+    dc->sk.calc_position_cb = dual_carriage_calc_position;
+    dc->sk.active_flags = orig_sk->active_flags;
+    dc->orig_sk = orig_sk;
+}
+
+int __visible
+dual_carriage_set_transform(struct stepper_kinematics *sk, char axis
+                            , double scale, double offs)
+{
+    struct dual_carriage_stepper *dc = container_of(
+            sk, struct dual_carriage_stepper, sk);
+    if (axis == 'x') {
+        dc->x_scale = scale;
+        dc->x_offs = offs;
+        if (!scale)
+            dc->sk.active_flags &= ~AF_X;
+        else if (scale && dc->orig_sk->active_flags & AF_X)
+            dc->sk.active_flags |= AF_X;
+        return 0;
+    }
+    if (axis == 'y') {
+        dc->y_scale = scale;
+        dc->y_offs = offs;
+        if (!scale)
+            dc->sk.active_flags &= ~AF_Y;
+        else if (scale && dc->orig_sk->active_flags & AF_Y)
+            dc->sk.active_flags |= AF_Y;
+        return 0;
+    }
+    return -1;
+}
+
+struct stepper_kinematics * __visible
+dual_carriage_alloc(void)
+{
+    struct dual_carriage_stepper *dc = malloc(sizeof(*dc));
+    memset(dc, 0, sizeof(*dc));
+    dc->m.move_t = 2. * DUMMY_T;
+    return &dc->sk;
+}
diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c
index 8c26e9a2..18d14143 100644
--- a/klippy/chelper/kin_shaper.c
+++ b/klippy/chelper/kin_shaper.c
@@ -204,11 +204,11 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
     struct input_shaper *is = container_of(sk, struct input_shaper, sk);
     struct shaper_pulses *sp = axis == 'x' ? &is->sx : &is->sy;
     int status = 0;
-    if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y))
+    // Ignore input shaper update if the axis is not active
+    if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y)) {
         status = init_shaper(n, a, t, sp);
-    else
-        sp->num_pulses = 0;
-    shaper_note_generation_time(is);
+        shaper_note_generation_time(is);
+    }
     return status;
 }