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 <[email protected]>

klippy/kinematics/idex_modes.py

@@ -4,7 +4,7 @@
 # Copyright (C) 2023  Dmitry Butyugin <[email protected]>
 #
 # 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 @@ def cmd_RESTORE_DUAL_CARRIAGE_STATE(self, gcmd):
         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 @@ def inactivate(self, position):
         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()

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

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