From e9181c435ce28d248a98a2a145be22ee1b4e8204 Mon Sep 17 00:00:00 2001
From: studiodyne <diabolomix@hotmail.fr>
Date: Sat, 2 Dec 2023 12:34:47 +0100
Subject: [PATCH] fix toolchange

---
 Marlin/src/module/tool_change.cpp | 23 ++++++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 22982fa91a44c..9c40eef39477c 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -1075,12 +1075,26 @@ void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0.
             destination.y = current_position.y;
           #endif
           do_blocking_move_to_xy(destination, MMM_TO_MMS(TOOLCHANGE_PARK_XY_FEEDRATE));
-          do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
-          planner.synchronize();
         }
       #endif
 
-      extruder_cutting_recover(destination.e); // Cutting recover
+      if (ok) do_blocking_move_to_z(destination.z, planner.settings.max_feedrate_mm_s[Z_AXIS]);
+      planner.synchronize();
+
+      // Clone previous position
+      planner.set_e_position_mm(current_position.e = destination.e);
+      extruder_cutting_recover(current_position.e); // Cutting recover
+
+      // Retract if previously retracted
+      #if ENABLED(FWRETRACT)
+        if (fwretract.retracted[active_extruder])
+          unscaled_e_move(-fwretract.settings.retract_length, fwretract.settings.retract_feedrate_mm_s);
+      #endif
+
+      // If resume_position is negative
+      if (current_position.e < 0) unscaled_e_move(current_position.e, MMM_TO_MMS(toolchange_settings.retract_speed));
+      planner.synchronize();
+      planner.set_e_position_mm(current_position.e); // Extruder primed and ready
     }
   }
 
@@ -1597,6 +1611,9 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
         unscaled_e_move(-fwretract.settings.retract_length, fwretract.settings.retract_feedrate_mm_s);
     #endif
 
+    // If resume_position is negative
+    if (resume_current_e < 0) unscaled_e_move(resume_current_e, MMM_TO_MMS(toolchange_settings.retract_speed));
+
     // If no available extruder
     if (EXTRUDERS < 2 || active_extruder >= EXTRUDERS - 2 || active_extruder == migration.last)
       migration.automode = false;