diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index cb74c59137..706ff433ab 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -494,21 +494,25 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n // Move to the "old position" (move the extruder into place) if (!no_move && IsRunning()) { + #if ENABLED(SWITCHING_NOZZLE) + if (z_raise != z_diff) + destination[Z_AXIS] += z_diff; // Include the Z restore with the "move back" + #endif #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination); #endif - prepare_move_to_destination(); + // Move back to the original (or tweaked) position + do_blocking_move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS]); } - #if ENABLED(SWITCHING_NOZZLE) // Move back down, if needed. (Including when the new tool is higher.) - if (z_raise != z_diff) { + else if (z_raise != z_diff) { + set_destination_from_current(); // Prevent any XY move destination[Z_AXIS] += z_diff; feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; prepare_move_to_destination(); } #endif - } // (tmp_extruder != active_extruder) stepper.synchronize();