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 92be95eb9f..021da6f3d4 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"
--
GitLab