From c753d2b7f4161e2fff30cc9061dba5e6d68a7762 Mon Sep 17 00:00:00 2001
From: qwewer0 <57561110+qwewer0@users.noreply.github.com>
Date: Mon, 9 Nov 2020 01:31:14 +0100
Subject: [PATCH] Use extra G35 BLTouch HS Mode clearance in Tramming Wizard
 (#20057)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
---
 Marlin/src/gcode/bedlevel/G35.cpp     |  3 +--
 Marlin/src/lcd/menu/menu_tramming.cpp |  4 ++++
 Marlin/src/module/probe.cpp           | 13 ++-----------
 3 files changed, 7 insertions(+), 13 deletions(-)

diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
index f0abc543cb..789d8bcf19 100755
--- a/Marlin/src/gcode/bedlevel/G35.cpp
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -120,8 +120,7 @@ void GcodeSuite::G35() {
     // In BLTOUCH HS mode, the probe travels in a deployed state.
     // Users of G35 might have a badly misaligned bed, so raise Z by the
     // length of the deployed pin (BLTOUCH stroke < 7mm)
-    current_position.z = (Z_CLEARANCE_BETWEEN_PROBES) + (7 * ENABLED(BLTOUCH_HS_MODE));
-
+    do_blocking_move_to_z((Z_CLEARANCE_BETWEEN_PROBES) + TERN0(BLTOUCH_HS_MODE, 7));
     const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true);
 
     if (isnan(z_probed_height)) {
diff --git a/Marlin/src/lcd/menu/menu_tramming.cpp b/Marlin/src/lcd/menu/menu_tramming.cpp
index a102fe7e57..317335d51b 100644
--- a/Marlin/src/lcd/menu/menu_tramming.cpp
+++ b/Marlin/src/lcd/menu/menu_tramming.cpp
@@ -43,6 +43,10 @@ float z_measured[G35_PROBE_COUNT] = { 0 };
 static uint8_t tram_index = 0;
 
 bool probe_single_point() {
+  // In BLTOUCH HS mode, the probe travels in a deployed state.
+  // Users of Tramming Wizard might have a badly misaligned bed, so raise Z by the
+  // length of the deployed pin (BLTOUCH stroke < 7mm)
+  do_blocking_move_to_z((Z_CLEARANCE_BETWEEN_PROBES) + TERN0(BLTOUCH_HS_MODE, 7));
   const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[tram_index], PROBE_PT_RAISE, 0, true);
   DEBUG_ECHOLNPAIR("probe_single_point: ", z_probed_height, "mm");
   z_measured[tram_index] = z_probed_height;
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index d008720389..ff80063d65 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -667,8 +667,8 @@ float Probe::probe_at_point(const float &rx, const float &ry, const ProbePtRaise
     if (bltouch.triggered()) bltouch._reset();
   #endif
 
-  // TODO: Adapt for SCARA, where the offset rotates
-  xyz_pos_t npos = { rx, ry };
+  // On delta keep Z below clip height or do_blocking_move_to will abort
+  xyz_pos_t npos = { rx, ry, _MIN(TERN(DELTA, delta_clip_start_height, current_position.z), current_position.z) };
   if (probe_relative) {                                     // The given position is in terms of the probe
     if (!can_reach(npos)) {
       if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
@@ -678,15 +678,6 @@ float Probe::probe_at_point(const float &rx, const float &ry, const ProbePtRaise
   }
   else if (!position_is_reachable(npos)) return NAN;        // The given position is in terms of the nozzle
 
-  npos.z =
-    #if ENABLED(DELTA)
-      // Move below clip height or xy move will be aborted by do_blocking_move_to
-      _MIN(current_position.z, delta_clip_start_height)
-    #else
-      current_position.z
-    #endif
-  ;
-
   const float old_feedrate_mm_s = feedrate_mm_s;
   feedrate_mm_s = XY_PROBE_FEEDRATE_MM_S;