From 0391e5bcc735d25c4618026f2656161c2631e592 Mon Sep 17 00:00:00 2001 From: Giuliano Zaro <3684609+GMagician@users.noreply.github.com> Date: Fri, 10 Jan 2020 23:12:21 +0100 Subject: [PATCH] Prevent Z misaligment on tool change (#16518) --- Marlin/src/module/tool_change.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index 92be95eb9..021da6f3d 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -929,11 +929,20 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) { #elif ENABLED(SWITCHING_NOZZLE) && !SWITCHING_NOZZLE_TWO_SERVOS // Switching Nozzle (single servo) // Raise by a configured distance to avoid workpiece, except with // SWITCHING_NOZZLE_TWO_SERVOS, as both nozzles will lift instead. - current_position.z += _MAX(-diff.z, 0.0) + toolchange_settings.z_raise; - #if HAS_SOFTWARE_ENDSTOPS - NOMORE(current_position.z, soft_endstop.max.z); - #endif - if (!no_move) fast_line_to_current(Z_AXIS); + if (!no_move) { + #if HAS_SOFTWARE_ENDSTOPS + const float maxz = _MIN(soft_endstop.max.z, Z_MAX_POS); + #else + constexpr float maxz = Z_MAX_POS; + #endif + + // Check if Z has space to compensate at least z_offset, and if not, just abort now + const float newz = current_position.z + _MAX(-diff.z, 0.0); + if (newz > maxz) return; + + current_position.z = _MIN(newz + toolchange_settings.z_raise, maxz); + fast_line_to_current(Z_AXIS); + } move_nozzle_servo(new_tool); #endif @@ -942,7 +951,7 @@ void tool_change(const uint8_t new_tool, bool no_move/*=false*/) { #endif // The newly-selected extruder XYZ is actually at... - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XY by { ", diff.x, ", ", diff.y, ", ", diff.z, " }"); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XYZ by { ", diff.x, ", ", diff.y, ", ", diff.z, " }"); current_position += diff; // Tell the planner the new "current position"