kinematics: Add dual_carriage to hybrid-corexyz (#4296)

- Add dual_carriage abilities for hybrid-corexy and hybrid-corexz
- Introduce the module idex_mode
- Fix add_stepper to the correct rail in hybrid-corexy

Signed-off-by: Fabrice GALLET <tircown@gmail.com>

6 files changed, 233 insertions, 9 deletions

diff --git a/docs/Status_Reference.md b/docs/Status_Reference.md
index da530e1b..e0da69e3 100644
--- a/docs/Status_Reference.md
+++ b/docs/Status_Reference.md
@@ -322,6 +322,15 @@ The following information is available in the `toolhead` object
   the printer had to be paused because the toolhead moved faster than
   moves could be read from the G-Code input.
 
+# dual_carriage
+
+The following information is available in
+[dual_carriage](Config_Reference.md#dual_carriage)
+on a hybrid_corexy or hybrid_corexz robot
+- `mode`: The current mode. Possible values are: "FULL_CONTROL"
+- `active_carriage`: The current active carriage.
+Possible values are: "CARRIAGE_0", "CARRIAGE_1"
+
 # virtual_sdcard
 
 The following information is available in the
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 7b20963d..e8760b54 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -80,6 +80,7 @@ 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 = """
diff --git a/klippy/chelper/kin_cartesian.c b/klippy/chelper/kin_cartesian.c
index 86569d4a..3b1c8cba 100644
--- a/klippy/chelper/kin_cartesian.c
+++ b/klippy/chelper/kin_cartesian.c
@@ -49,3 +49,42 @@ 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/kinematics/hybrid_corexy.py b/klippy/kinematics/hybrid_corexy.py
index 43cf7dd9..49489962 100644
--- a/klippy/kinematics/hybrid_corexy.py
+++ b/klippy/kinematics/hybrid_corexy.py
@@ -4,7 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging
-import stepper
+import stepper, idex_modes
 
 # The hybrid-corexy kinematic is also known as Markforged kinematics
 class HybridCoreXYKinematics:
@@ -15,7 +15,7 @@ class HybridCoreXYKinematics:
         self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
                        stepper.LookupMultiRail(config.getsection('stepper_y')),
                        stepper.LookupMultiRail(config.getsection('stepper_z'))]
-        self.rails[2].get_endstops()[0][0].add_stepper(
+        self.rails[1].get_endstops()[0][0].add_stepper(
             self.rails[0].get_steppers()[0])
         self.rails[0].setup_itersolve('corexy_stepper_alloc', '-')
         self.rails[1].setup_itersolve('cartesian_stepper_alloc', 'y')
@@ -23,6 +23,26 @@ class HybridCoreXYKinematics:
         ranges = [r.get_range() for r in self.rails]
         self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
         self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
+        self.dc_module = None
+        if config.has_section('dual_carriage'):
+            dc_config = config.getsection('dual_carriage')
+            # dummy for cartesian config users
+            dc_config.getchoice('axis', {'x': 'x'}, default='x')
+            # setup second dual carriage rail
+            self.rails.append(stepper.PrinterRail(dc_config))
+            self.rails[1].get_endstops()[0][0].add_stepper(
+                self.rails[3].get_steppers()[0])
+            self.rails[3].setup_itersolve('cartesian_stepper_alloc', 'y')
+            dc_rail_0 = idex_modes.DualCarriagesRail(
+                self.printer, self.rails[0], axis=0, active=True,
+                stepper_alloc_active=('corexy_stepper_alloc','-'),
+                stepper_alloc_inactive=('cartesian_reverse_stepper_alloc','y'))
+            dc_rail_1 = idex_modes.DualCarriagesRail(
+                self.printer, self.rails[3], axis=0, active=False,
+                stepper_alloc_active=('corexy_stepper_alloc','+'),
+                stepper_alloc_inactive=('cartesian_stepper_alloc','y'))
+            self.dc_module = idex_modes.DualCarriages(self.printer,
+                        dc_rail_0, dc_rail_1, axis=0)
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
             toolhead.register_step_generator(s.generate_steps)
@@ -39,7 +59,15 @@ class HybridCoreXYKinematics:
         return [s for rail in self.rails for s in rail.get_steppers()]
     def calc_position(self, stepper_positions):
         pos = [stepper_positions[rail.get_name()] for rail in self.rails]
-        return [pos[0] + pos[1], pos[1], pos[2]]
+        if (self.dc_module is not None and 'CARRIAGE_1' == \
+                    self.dc_module.get_status()['active_carriage']):
+            return [pos[0] - pos[1], pos[1], pos[2]]
+        else:
+            return [pos[0] + pos[1], pos[1], pos[2]]
+    def update_limits(self, i, range):
+        self.limits[i] = range
+    def override_rail(self, i, rail):
+        self.rails[i] = rail
     def set_position(self, newpos, homing_axes):
         for i, rail in enumerate(self.rails):
             rail.set_position(newpos)
@@ -62,7 +90,14 @@ class HybridCoreXYKinematics:
         homing_state.home_rails([rail], forcepos, homepos)
     def home(self, homing_state):
         for axis in homing_state.get_axes():
-            self._home_axis(homing_state, axis, self.rails[axis])
+            if (self.dc_module is not None and axis == 0):
+                self.dc_module.save_idex_state()
+                for i in [0,1]:
+                    self.dc_module.toggle_active_dc_rail(i)
+                    self._home_axis(homing_state, axis, self.rails[0])
+                self.dc_module.restore_idex_state()
+            else:
+                self._home_axis(homing_state, axis, self.rails[axis])
     def _motor_off(self, print_time):
         self.limits = [(1.0, -1.0)] * 3
     def _check_endstops(self, move):
@@ -93,7 +128,7 @@ class HybridCoreXYKinematics:
         return {
             'homed_axes': "".join(axes),
             'axis_minimum': self.axes_min,
-            'axis_maximum': self.axes_max,
+            'axis_maximum': self.axes_max
         }
 
 def load_kinematics(toolhead, config):
diff --git a/klippy/kinematics/hybrid_corexz.py b/klippy/kinematics/hybrid_corexz.py
index 47aa430e..e6e471ab 100644
--- a/klippy/kinematics/hybrid_corexz.py
+++ b/klippy/kinematics/hybrid_corexz.py
@@ -4,7 +4,7 @@
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging
-import stepper
+import stepper, idex_modes
 
 # The hybrid-corexz kinematic is also known as Markforged kinematics
 class HybridCoreXZKinematics:
@@ -23,6 +23,26 @@ class HybridCoreXZKinematics:
         ranges = [r.get_range() for r in self.rails]
         self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
         self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
+        self.dc_module = None
+        if config.has_section('dual_carriage'):
+            dc_config = config.getsection('dual_carriage')
+            # dummy for cartesian config users
+            dc_config.getchoice('axis', {'x': 'x'}, default='x')
+            # setup second dual carriage rail
+            self.rails.append(stepper.PrinterRail(dc_config))
+            self.rails[2].get_endstops()[0][0].add_stepper(
+                self.rails[3].get_steppers()[0])
+            self.rails[3].setup_itersolve('cartesian_stepper_alloc', 'z')
+            dc_rail_0 = idex_modes.DualCarriagesRail(
+                self.printer, self.rails[0], axis=0, active=True,
+                stepper_alloc_active=('corexz_stepper_alloc','-'),
+                stepper_alloc_inactive=('cartesian_reverse_stepper_alloc','z'))
+            dc_rail_1 = idex_modes.DualCarriagesRail(
+                self.printer, self.rails[3], axis=0, active=False,
+                stepper_alloc_active=('corexz_stepper_alloc','+'),
+                stepper_alloc_inactive=('cartesian_stepper_alloc','z'))
+            self.dc_module = idex_modes.DualCarriages(self.printer,
+                        dc_rail_0, dc_rail_1, axis=0)
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
             toolhead.register_step_generator(s.generate_steps)
@@ -39,7 +59,15 @@ class HybridCoreXZKinematics:
         return [s for rail in self.rails for s in rail.get_steppers()]
     def calc_position(self, stepper_positions):
         pos = [stepper_positions[rail.get_name()] for rail in self.rails]
-        return [pos[0] + pos[2], pos[1], pos[2]]
+        if (self.dc_module is not None and 'CARRIAGE_1' == \
+                    self.dc_module.get_status()['active_carriage']):
+            return [pos[0] - pos[2], pos[1], pos[2]]
+        else:
+            return [pos[0] + pos[2], pos[1], pos[2]]
+    def update_limits(self, i, range):
+        self.limits[i] = range
+    def override_rail(self, i, rail):
+        self.rails[i] = rail
     def set_position(self, newpos, homing_axes):
         for i, rail in enumerate(self.rails):
             rail.set_position(newpos)
@@ -62,7 +90,14 @@ class HybridCoreXZKinematics:
         homing_state.home_rails([rail], forcepos, homepos)
     def home(self, homing_state):
         for axis in homing_state.get_axes():
-            self._home_axis(homing_state, axis, self.rails[axis])
+            if (self.dc_module is not None and axis == 0):
+                self.dc_module.save_idex_state()
+                for i in [0,1]:
+                    self.dc_module.toggle_active_dc_rail(i)
+                    self._home_axis(homing_state, axis, self.rails[0])
+                self.dc_module.restore_idex_state()
+            else:
+                self._home_axis(homing_state, axis, self.rails[axis])
     def _motor_off(self, print_time):
         self.limits = [(1.0, -1.0)] * 3
     def _check_endstops(self, move):
@@ -93,7 +128,7 @@ class HybridCoreXZKinematics:
         return {
             'homed_axes': "".join(axes),
             'axis_minimum': self.axes_min,
-            'axis_maximum': self.axes_max,
+            'axis_maximum': self.axes_max
         }
 
 def load_kinematics(toolhead, config):
diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py
new file mode 100644
index 00000000..92cb39a3
--- /dev/null
+++ b/klippy/kinematics/idex_modes.py
@@ -0,0 +1,105 @@
+# Support for duplication and mirroring modes for IDEX printers
+#
+# Copyright (C) 2021  Fabrice Gallet <tircown@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import math
+
+class DualCarriages:
+    def __init__(self, printer, rail_0, rail_1, axis):
+        self.printer = printer
+        self.axis = axis
+        self.dc = (rail_0, rail_1)
+        self.saved_state = None
+        self.printer.add_object('dual_carriage', self)
+        gcode = self.printer.lookup_object('gcode')
+        gcode.register_command(
+                   'SET_DUAL_CARRIAGE', self.cmd_SET_DUAL_CARRIAGE,
+                   desc=self.cmd_SET_DUAL_CARRIAGE_help)
+    def toggle_active_dc_rail(self, index):
+        toolhead = self.printer.lookup_object('toolhead')
+        toolhead.flush_step_generation()
+        pos = toolhead.get_position()
+        kin = toolhead.get_kinematics()
+        for i, dc in enumerate(self.dc):
+            dc_rail = dc.get_rail()
+            if i != index:
+                dc.inactivate(pos)
+                kin.override_rail(3, dc_rail)
+            elif dc.is_active() is False:
+                newpos = pos[:self.axis] + [dc.axis_position] \
+                            + pos[self.axis+1:]
+                dc.activate(newpos)
+                kin.override_rail(self.axis, dc_rail)
+                toolhead.set_position(newpos)
+                kin.update_limits(self.axis, dc_rail.get_range())
+    def get_status(self, eventtime):
+        dc0, dc1 = self.dc
+        if (dc0.is_active() is True):
+            return { 'mode': 'FULL_CONTROL', 'active_carriage': 'CARRIAGE_0' }
+        else:
+            return { 'mode': 'FULL_CONTROL', 'active_carriage': 'CARRIAGE_1' }
+    def save_idex_state(self):
+        dc0, dc1 = self.dc
+        if (dc0.is_active() is True):
+            mode, active_carriage = ('FULL_CONTROL', 'CARRIAGE_0')
+        else:
+            mode, active_carriage = ('FULL_CONTROL', 'CARRIAGE_1')
+        self.saved_state = {
+            'mode': mode,
+            'active_carriage': active_carriage,
+            'axis_positions': (dc0.axis_position, dc1.axis_position)
+            }
+    def restore_idex_state(self):
+        if self.saved_state is not None:
+            # set carriage 0 active
+            if (self.saved_state['active_carriage'] == 'CARRIAGE_0'
+                        and self.dc[0].is_active() is False):
+                self.toggle_active_dc_rail(0)
+            # set carriage 1 active
+            elif (self.saved_state['active_carriage'] == 'CARRIAGE_1'
+                        and self.dc[1].is_active() is False):
+                self.toggle_active_dc_rail(1)
+    cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"
+    def cmd_SET_DUAL_CARRIAGE(self, gcmd):
+        index = gcmd.get_int('CARRIAGE', minval=0, maxval=1)
+        if (not(self.dc[0].is_active() == self.dc[1].is_active() == True)
+                    and self.dc[index].is_active() is False):
+            self.toggle_active_dc_rail(index)
+
+class DualCarriagesRail:
+    ACTIVE=1
+    INACTIVE=2
+    def __init__(self, printer, rail, axis, active, stepper_alloc_active,
+                 stepper_alloc_inactive=None):
+        self.printer = printer
+        self.rail = rail
+        self.axis = axis
+        self.status = (self.INACTIVE, self.ACTIVE)[active]
+        self.stepper_alloc_active = stepper_alloc_active
+        self.stepper_alloc_inactive = stepper_alloc_inactive
+        self.axis_position = -1
+    def _stepper_alloc(self, position, active=True):
+        toolhead = self.printer.lookup_object('toolhead')
+        self.axis_position = position[self.axis]
+        self.rail.set_trapq(None)
+        if active is True:
+            self.status = self.ACTIVE
+            if self.stepper_alloc_active is not None:
+                self.rail.setup_itersolve(*self.stepper_alloc_active)
+                self.rail.set_position(position)
+                self.rail.set_trapq(toolhead.get_trapq())
+        else:
+            self.status = self.INACTIVE
+            if self.stepper_alloc_inactive is not None:
+                self.rail.setup_itersolve(*self.stepper_alloc_inactive)
+                self.rail.set_position(position)
+                self.rail.set_trapq(toolhead.get_trapq())
+    def get_rail(self):
+        return self.rail
+    def is_active(self):
+        return self.status == self.ACTIVE
+    def activate(self, position):
+        self._stepper_alloc(position, active=True)
+    def inactivate(self, position):
+        self._stepper_alloc(position, active=False)