From 21a47b50f4f6489759e2430bd6273cd7dbd0df82 Mon Sep 17 00:00:00 2001
From: GMagician <3684609+GMagician@users.noreply.github.com>
Date: Thu, 5 Apr 2018 00:06:33 +0200
Subject: [PATCH] Additional tool-change fixes (#10304)

Followup to #10257
---
 Marlin/src/module/tool_change.cpp | 23 +++++++++++++----------
 1 file changed, 13 insertions(+), 10 deletions(-)

diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index bbed2cafb3..2bb071a20e 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -398,9 +398,9 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
           #endif
 
           #if ENABLED(SWITCHING_NOZZLE)
-            // Always raise by at least 0.3
-            const float z_diff = hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder];
-            current_position[Z_AXIS] += (z_diff > 0.0 ? z_diff : 0.0) + 0.3;
+            // Always raise by at least 1 to avoid workpiece
+            const float zdiff = hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder];
+            current_position[Z_AXIS] += (zdiff > 0.0 ? zdiff : 0.0) + 1;
             planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
             move_nozzle_servo(tmp_extruder);
           #endif
@@ -430,6 +430,11 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
           set_bed_leveling_enabled(leveling_was_active);
         #endif
 
+        #if ENABLED(SWITCHING_NOZZLE)
+          // The newly-selected extruder Z is actually at...
+          current_position[Z_AXIS] -= zdiff;
+        #endif
+
         // Tell the planner the new "current position"
         SYNC_PLAN_POSITION_KINEMATIC();
 
@@ -440,15 +445,13 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
           constexpr bool safe_to_move = true;
         #endif
 
-        #if ENABLED(SWITCHING_NOZZLE)
-          destination[Z_AXIS] += z_diff;  // Include the Z restore with the "move back"
-        #endif
-
         // Raise, move, and lower again
         if (safe_to_move && !no_move && IsRunning()) {
-          // Do a small lift to avoid the workpiece in the move back (below)
-          current_position[Z_AXIS] += 1.0;
-          planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
+          #if DISABLED(SWITCHING_NOZZLE)
+            // Do a small lift to avoid the workpiece in the move back (below)
+            current_position[Z_AXIS] += 1.0;
+            planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
+          #endif
           #if ENABLED(DEBUG_LEVELING_FEATURE)
             if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);
           #endif