Refactor 'Level Corners with Probe' (#20460)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/src/feature/bltouch.h

@@ -23,6 +23,10 @@
 
 #include "../inc/MarlinConfigPre.h"
 
+#if DISABLED(BLTOUCH_HS_MODE)
+  #define BLTOUCH_SLOW_MODE 1
+#endif
+
 // BLTouch commands are sent as servo angles
 typedef unsigned char BLTCommand;
 

Marlin/src/lcd/menu/menu_bed_corners.cpp

@@ -51,18 +51,11 @@
     #include "../../feature/bltouch.h"
   #endif
   #ifndef LEVEL_CORNERS_PROBE_TOLERANCE
-    #define LEVEL_CORNERS_PROBE_TOLERANCE 0.1
+    #define LEVEL_CORNERS_PROBE_TOLERANCE 0.2
   #endif
-  #if ENABLED(LEVEL_CORNERS_AUDIO_FEEDBACK)
+  float last_z;
-    #include "../../libs/buzzer.h"
+  int good_points;
-    #define PROBE_BUZZ() BUZZ(200, 600)
+  bool corner_probing_done, wait_for_probe;
-  #else
-    #define PROBE_BUZZ() NOOP
-  #endif
-  static float last_z;
-  static bool corner_probing_done;
-  static bool verify_corner;
-  static int good_points;
 #endif
 
 static_assert(LEVEL_CORNERS_Z_HOP >= 0, "LEVEL_CORNERS_Z_HOP must be >= 0. Please update your configuration.");
@@ -75,115 +68,130 @@ extern const char G28_STR[];
 
 static int8_t bed_corner;
 
+constexpr float inset_lfrb[4] = LEVEL_CORNERS_INSET_LFRB;
+constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1] },
+                   rb { (X_MAX_BED) - inset_lfrb[2], (Y_MAX_BED) - inset_lfrb[3] };
+
 /**
  * Level corners, starting in the front-left corner.
  */
 #if ENABLED(LEVEL_CORNERS_USE_PROBE)
 
-  static inline void _lcd_level_bed_corners_probing() {
+  void _lcd_draw_probing() {
-    ui.goto_screen([]{ MenuItem_static::draw((LCD_HEIGHT - 1) / 2, GET_TEXT(MSG_PROBING_MESH)); });
+    if (ui.should_draw()) MenuItem_static::draw((LCD_HEIGHT - 1) / 2, GET_TEXT(MSG_PROBING_MESH));
-
-    float lfrb[4] = LEVEL_CORNERS_INSET_LFRB;
-    xy_pos_t lf { (X_MIN_BED) + lfrb[0] - probe.offset_xy.x , (Y_MIN_BED) + lfrb[1] - probe.offset_xy.y },
-             rb { (X_MAX_BED) - lfrb[2] - probe.offset_xy.x , (Y_MAX_BED) - lfrb[3] - probe.offset_xy.y };
-
-    do_blocking_move_to_z(LEVEL_CORNERS_Z_HOP - probe.offset.z);
-
-    switch (bed_corner) {
-      case 0: current_position   = lf;   break; // copy xy
-      case 1: current_position.x = rb.x; break;
-      case 2: current_position.y = rb.y; break;
-      case 3: current_position.x = lf.x; break;
-      #if ENABLED(LEVEL_CENTER_TOO)
-        case 4: current_position.set(X_CENTER - probe.offset_xy.x, Y_CENTER - probe.offset_xy.y); good_points--; break;
-      #endif
   }
 
-    do_blocking_move_to_xy(current_position);
+  void _lcd_draw_raise() {
-
+    if (!ui.should_draw()) return;
-    #if ENABLED(BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
-      bltouch.deploy(); // DEPLOY in LOW SPEED MODE on every probe action
-    #endif
-    TERN_(HAS_QUIET_PROBING, probe.set_probing_paused(true));
-
-    // Move down until the probe is triggered
-    do_blocking_move_to_z(last_z - (LEVEL_CORNERS_PROBE_TOLERANCE), manual_feedrate_mm_s.z);
-
-    // Check to see if the probe was triggered
-    bool probe_triggered = TEST(endstops.trigger_state(), TERN(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, Z_MIN, Z_MIN_PROBE));
-    if (!probe_triggered) {
-
-      static bool wait_for_probe;
-
-      ui.goto_screen([]{
     MenuItem_confirm::select_screen(
       GET_TEXT(MSG_BUTTON_DONE), GET_TEXT(MSG_BUTTON_SKIP)
-          , []{ corner_probing_done = true;
+      , []{ corner_probing_done = true; wait_for_probe = false; }
-                wait_for_probe = false;
-                TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_was_active));
-                ui.goto_previous_screen_no_defer();
-            }
       , []{ wait_for_probe = false; }
       , GET_TEXT(MSG_LEVEL_CORNERS_RAISE)
       , (const char*)nullptr, PSTR("")
     );
-      });
-      ui.set_selection(true);
-
-      wait_for_probe = true;
-      while (wait_for_probe && !probe_triggered) {
-        probe_triggered = PROBE_TRIGGERED();
-        if (probe_triggered) PROBE_BUZZ();
-        idle();
-      }
-      wait_for_probe = false;
-
-      TERN_(LEVEL_CORNERS_VERIFY_RAISED, verify_corner = true);
   }
 
-    TERN_(HAS_QUIET_PROBING, probe.set_probing_paused(false));
+  void _lcd_draw_level_prompt() {
-
+    if (!ui.should_draw()) return;
-    #if ENABLED(BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
-      bltouch.stow();
-    #endif
-
-    if (probe_triggered) {
-      endstops.hit_on_purpose();
-      if (!WITHIN(current_position.z, last_z - (LEVEL_CORNERS_PROBE_TOLERANCE), last_z + (LEVEL_CORNERS_PROBE_TOLERANCE))) {
-        last_z = current_position.z;
-        good_points = 0;
-      }
-      if (!verify_corner) good_points++;
-    }
-
-    if (!corner_probing_done) {
-      if (!verify_corner) bed_corner++;
-      if (bed_corner > 3) bed_corner = 0;
-      verify_corner = false;
-      if (good_points < 4)
-        _lcd_level_bed_corners_probing();
-      else {
-        ui.goto_screen([]{
     MenuItem_confirm::confirm_screen(
-            []{ ui.goto_previous_screen_no_defer();
+      []{ queue.inject_P(TERN(HAS_LEVELING, PSTR("G28\nG29"), G28_STR));
-                queue.inject_P(TERN(HAS_LEVELING, PSTR("G28\nG29"), G28_STR));
+          ui.return_to_status();
       }
       , []{ ui.goto_previous_screen_no_defer(); }
       , GET_TEXT(MSG_LEVEL_CORNERS_IN_RANGE)
       , (const char*)nullptr, PSTR("?")
     );
-        });
+  }
+
+  bool _lcd_level_bed_corners_probe(bool verify=false) {
+    if (verify) do_blocking_move_to_z(current_position.z + LEVEL_CORNERS_Z_HOP); // do clearance if needed
+    TERN_(BLTOUCH_SLOW_MODE, bltouch.deploy()); // Deploy in LOW SPEED MODE on every probe action
+    do_blocking_move_to_z(last_z - LEVEL_CORNERS_PROBE_TOLERANCE, manual_feedrate_mm_s.z); // Move down to lower tolerance
+    if (TEST(endstops.trigger_state(), TERN(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, Z_MIN, Z_MIN_PROBE))) { // check if probe triggered
+      endstops.hit_on_purpose();
+      set_current_from_steppers_for_axis(Z_AXIS);
+      sync_plan_position();
+      TERN_(BLTOUCH_SLOW_MODE, bltouch.stow()); // Stow in LOW SPEED MODE on every trigger
+      // Triggered outside tolerance range?
+      if (ABS(current_position.z - last_z) > LEVEL_CORNERS_PROBE_TOLERANCE) {
+        last_z = current_position.z; // Above tolerance. Set a new Z for subsequent corners.
+        good_points = 0;             // ...and start over
+      }
+      return true; // probe triggered
+    }
+    do_blocking_move_to_z(last_z); // go back to tolerance middle point before raise
+    return false; // probe not triggered
+  }
+
+  bool _lcd_level_bed_corners_raise() {
+    bool probe_triggered = false;
+    corner_probing_done = false;
+    wait_for_probe = true;
+    ui.goto_screen(_lcd_draw_raise); // show raise screen
+    ui.set_selection(true);
+    while (wait_for_probe && !probe_triggered) { //loop while waiting to bed raise and probe trigger
+      probe_triggered = PROBE_TRIGGERED();
+      if (probe_triggered) {
+        endstops.hit_on_purpose();
+        TERN_(LEVEL_CORNERS_AUDIO_FEEDBACK, ui.buzz(200, 600));
+      }
+      idle();
+    }
+    TERN_(BLTOUCH_SLOW_MODE, bltouch.stow());
+    ui.goto_screen(_lcd_draw_probing);
+    return (probe_triggered);
+  }
+
+  void _lcd_test_corners() {
+    ui.goto_screen(_lcd_draw_probing);
+    bed_corner = TERN(LEVEL_CENTER_TOO, 4, 0);
+    last_z = LEVEL_CORNERS_HEIGHT;
+    endstops.enable_z_probe(true);
+    good_points = 0;
+
+    do {
+      do_blocking_move_to_z(current_position.z + LEVEL_CORNERS_Z_HOP); // clearance
+      // Select next corner coordinates
+      xy_pos_t plf = lf - probe.offset_xy, prb = rb - probe.offset_xy;
+      switch (bed_corner) {
+        case 0: current_position   = plf;   break; // copy xy
+        case 1: current_position.x = prb.x; break;
+        case 2: current_position.y = prb.y; break;
+        case 3: current_position.x = plf.x; break;
+        #if ENABLED(LEVEL_CENTER_TOO)
+          case 4: current_position.set(X_CENTER - probe.offset_xy.x, Y_CENTER - probe.offset_xy.y); break;
+        #endif
+      }
+      do_blocking_move_to_xy(current_position);           // Goto corner
+
+      if (!_lcd_level_bed_corners_probe()) {              // Probe down to tolerance
+        if (_lcd_level_bed_corners_raise()) {             // Prompt user to raise bed if needed
+          #if ENABLED(LEVEL_CORNERS_VERIFY_RAISED)        // Verify
+            while (!_lcd_level_bed_corners_probe(true)) { // Loop while corner verified
+              if (!_lcd_level_bed_corners_raise()) {      // Prompt user to raise bed if needed
+                if (corner_probing_done) return;          // Done was selected
+                break;                                    // Skip was selected
+              }
+            }
+          #endif
+        }
+        else if (corner_probing_done)                     // Done was selected
+          return;
+      }
+
+      if (bed_corner != 4) good_points++; // ignore center
+      if (++bed_corner > 3) bed_corner = 0;
+
+    } while (good_points < 4); // loop until all corners whitin tolerance
+
+    ui.goto_screen(_lcd_draw_level_prompt); // prompt for bed leveling
     ui.set_selection(true);
   }
-    }
-  }
 
-#else
+#else // !LEVEL_CORNERS_USE_PROBE
 
   static inline void _lcd_goto_next_corner() {
-    constexpr float lfrb[4] = LEVEL_CORNERS_INSET_LFRB;
-    constexpr xy_pos_t lf { (X_MIN_BED) + lfrb[0], (Y_MIN_BED) + lfrb[1] },
-                       rb { (X_MAX_BED) - lfrb[2], (Y_MAX_BED) - lfrb[3] };
     line_to_z(LEVEL_CORNERS_Z_HOP);
     switch (bed_corner) {
       case 0: current_position   = lf;   break; // copy xy
@@ -199,15 +207,16 @@ static int8_t bed_corner;
     if (++bed_corner > 3 + ENABLED(LEVEL_CENTER_TOO)) bed_corner = 0;
   }
 
-#endif
+#endif // !LEVEL_CORNERS_USE_PROBE
 
 static inline void _lcd_level_bed_corners_homing() {
   _lcd_draw_homing();
-  if (all_axes_homed()) {
+  if (!all_axes_homed()) return;
   #if ENABLED(LEVEL_CORNERS_USE_PROBE)
-      TERN_(LEVEL_CENTER_TOO, bed_corner = 4);
+    _lcd_test_corners();
-      endstops.enable_z_probe(true);
+    if (corner_probing_done) ui.goto_previous_screen_no_defer();
-      ui.goto_screen(_lcd_level_bed_corners_probing);
+    TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_was_active));
+    endstops.enable_z_probe(false);
   #else
     bed_corner = 0;
     ui.goto_screen([]{
@@ -225,7 +234,6 @@ static inline void _lcd_level_bed_corners_homing() {
     ui.set_selection(true);
     _lcd_goto_next_corner();
   #endif
-  }
 }
 
 void _lcd_level_bed_corners() {
@@ -241,13 +249,6 @@ void _lcd_level_bed_corners() {
     set_bed_leveling_enabled(false);
   #endif
 
-  #if ENABLED(LEVEL_CORNERS_USE_PROBE)
-    last_z = LEVEL_CORNERS_HEIGHT;
-    corner_probing_done = false;
-    verify_corner = false;
-    good_points = 0;
-  #endif
-
   ui.goto_screen(_lcd_level_bed_corners_homing);
 }
 

Marlin/src/module/motion.cpp

@@ -1604,7 +1604,7 @@ void homeaxis(const AxisEnum axis) {
 
   do_homing_move(axis, 1.5f * max_length(TERN(DELTA, Z_AXIS, axis)) * axis_home_dir);
 
-  #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
+  #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
     if (axis == Z_AXIS) bltouch.stow(); // Intermediate STOW (in LOW SPEED MODE)
   #endif
 
@@ -1642,7 +1642,7 @@ void homeaxis(const AxisEnum axis) {
     // Slow move towards endstop until triggered
     if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Home 2 Slow:");
 
-    #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
+    #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
       if (axis == Z_AXIS && bltouch.deploy()) return; // Intermediate DEPLOY (in LOW SPEED MODE)
     #endif
 

Marlin/src/module/probe.cpp

@@ -455,9 +455,7 @@ bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
     thermalManager.wait_for_bed_heating();
   #endif
 
-  #if ENABLED(BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
+  if (TERN0(BLTOUCH_SLOW_MODE, bltouch.deploy())) return true; // Deploy in LOW SPEED MODE on every probe action
-    if (bltouch.deploy()) return true; // DEPLOY in LOW SPEED MODE on every probe action
-  #endif
 
   // Disable stealthChop if used. Enable diag1 pin on driver.
   #if ENABLED(SENSORLESS_PROBING)
@@ -496,9 +494,8 @@ bool Probe::probe_down_to_z(const float z, const feedRate_t fr_mm_s) {
     tmc_disable_stallguard(stepperZ, stealth_states.z);
   #endif
 
-  #if ENABLED(BLTOUCH) && DISABLED(BLTOUCH_HS_MODE)
+  if (probe_triggered && TERN0(BLTOUCH_SLOW_MODE, bltouch.stow())) // Stow in LOW SPEED MODE on every trigger
-    if (probe_triggered && bltouch.stow()) return true; // STOW in LOW SPEED MODE on trigger on every probe action
+    return true;
-  #endif
 
   // Clear endstop flags
   endstops.hit_on_purpose();
@@ -578,9 +575,10 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
   // Double-probing does a fast probe followed by a slow probe
   #if TOTAL_PROBING == 2
 
-    // Do a first probe at the fast speed
+    // Attempt to tare the probe
     if (TERN0(PROBE_TARE, tare())) return NAN;
 
+    // Do a first probe at the fast speed
     if (try_to_probe(PSTR("FAST"), z_probe_low_point, z_probe_fast_mm_s,
                      sanity_check, Z_CLEARANCE_BETWEEN_PROBES) ) return NAN;