idex_modes: Improved restoring position in RESTORE_DUAL_CARRIAGE_STATE

Previous implementation could crash the idex carriages into each other.
The new code moves the idex carriages together, eliminating this risk
and decreasing the time needed to restore the carriages positions.

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

3 files changed, 41 insertions, 7 deletions

diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py
index f2618d08..2f2da416 100644
--- a/klippy/kinematics/idex_modes.py
+++ b/klippy/kinematics/idex_modes.py
@@ -4,7 +4,7 @@
 # Copyright (C) 2023  Dmitry Butyugin <dmbutyugin@google.com>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-import math
+import logging, math
 import chelper
 
 INACTIVE = 'INACTIVE'
@@ -202,14 +202,31 @@ class DualCarriages:
         move_speed = gcmd.get_float('MOVE_SPEED', 0., above=0.)
         toolhead = self.printer.lookup_object('toolhead')
         toolhead.flush_step_generation()
-        pos = toolhead.get_position()
         if gcmd.get_int('MOVE', 1):
+            homing_speed = 99999999.
+            cur_pos = []
             for i, dc in enumerate(self.dc):
                 self.toggle_active_dc_rail(i)
-                saved_pos = saved_state['axes_positions'][i]
-                toolhead.manual_move(
-                        pos[:self.axis] + [saved_pos] + pos[self.axis+1:],
-                        move_speed or dc.get_rail().homing_speed)
+                homing_speed = min(homing_speed, dc.get_rail().homing_speed)
+                cur_pos.append(toolhead.get_position())
+            move_pos = list(cur_pos[0])
+            dl = [saved_state['axes_positions'][i] - cur_pos[i][self.axis]
+                  for i in range(2)]
+            primary_ind = 0 if abs(dl[0]) >= abs(dl[1]) else 1
+            self.toggle_active_dc_rail(primary_ind)
+            move_pos[self.axis] = saved_state['axes_positions'][primary_ind]
+            dc_mode = INACTIVE if min(abs(dl[0]), abs(dl[1])) < 0.000000001 \
+                    else COPY if dl[0] * dl[1] > 0 else MIRROR
+            if dc_mode != INACTIVE:
+                self.dc[1-primary_ind].activate(dc_mode, cur_pos[primary_ind])
+                self.dc[1-primary_ind].override_axis_scaling(
+                        abs(dl[1-primary_ind] / dl[primary_ind]),
+                        cur_pos[primary_ind])
+            toolhead.manual_move(move_pos, move_speed or homing_speed)
+            toolhead.flush_step_generation()
+            # Make sure the scaling coefficients are restored with the mode
+            self.dc[0].inactivate(move_pos)
+            self.dc[1].inactivate(move_pos)
         for i, dc in enumerate(self.dc):
             saved_mode = saved_state['carriage_modes'][i]
             self.activate_dc_mode(i, saved_mode)
@@ -257,3 +274,8 @@ class DualCarriagesRail:
         self.scale = 0.
         self.apply_transform()
         self.mode = INACTIVE
+    def override_axis_scaling(self, new_scale, position):
+        old_axis_position = self.get_axis_position(position)
+        self.scale = math.copysign(new_scale, self.scale)
+        self.offset = old_axis_position - position[self.axis] * self.scale
+        self.apply_transform()
diff --git a/test/klippy/dual_carriage.cfg b/test/klippy/dual_carriage.cfg
index 9ae01c2b..93c57444 100644
--- a/test/klippy/dual_carriage.cfg
+++ b/test/klippy/dual_carriage.cfg
@@ -61,7 +61,7 @@ pid_Kd: 114
 min_temp: 0
 max_temp: 250
 
-[gcode_macro PARK_extruder0]
+[gcode_macro PARK_extruder]
 gcode:
     G90
     G1 X0
diff --git a/test/klippy/dual_carriage.test b/test/klippy/dual_carriage.test
index 5b2f9e65..ed40c236 100644
--- a/test/klippy/dual_carriage.test
+++ b/test/klippy/dual_carriage.test
@@ -17,6 +17,18 @@ G1 X190 F6000
 SET_DUAL_CARRIAGE CARRIAGE=0
 G1 X20 F6000
 
+# Save dual carriage state
+SAVE_DUAL_CARRIAGE_STATE
+
+G1 X50 F6000
+
+# Go back to alternate carriage
+SET_DUAL_CARRIAGE CARRIAGE=1
+G1 X170 F6000
+
+# Restore dual carriage state
+RESTORE_DUAL_CARRIAGE_STATE
+
 # Test changing extruders
 G1 X5
 T1