From 43a91e5963c7563c4db3fcde4f4862ea0f9670ee Mon Sep 17 00:00:00 2001
From: RFBomb <Robbery525@gmail.com>
Date: Sat, 16 Jan 2021 01:43:38 -0500
Subject: [PATCH] Configurable Corner Leveling point order (#20733)

---
 Marlin/Configuration.h                   |  19 +++
 Marlin/src/lcd/language/language_en.h    |   2 +
 Marlin/src/lcd/menu/menu_bed_corners.cpp | 159 ++++++++++++++++++-----
 buildroot/tests/rambo-tests              |   6 +-
 4 files changed, 155 insertions(+), 31 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 3f9b6174c2f..4ec64f8659d 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -1451,6 +1451,25 @@
     #define LEVEL_CORNERS_VERIFY_RAISED   // After adjustment triggers the probe, re-probe to verify
     //#define LEVEL_CORNERS_AUDIO_FEEDBACK
   #endif
+
+  /**
+   * Corner Leveling Order
+   *
+   * Set 2 or 4 points. When 2 points are given, the 3rd is the center of the opposite edge.
+   *
+   *  LF  Left-Front    RF  Right-Front
+   *  LB  Left-Back     RB  Right-Back
+   *
+   * Examples:
+   *
+   *      Default        {LF,RB,LB,RF}         {LF,RF}           {LB,LF}
+   *  LB --------- RB   LB --------- RB    LB --------- RB   LB --------- RB
+   *  |  4       3  |   | 3         2 |    |     <3>     |   | 1           |
+   *  |             |   |             |    |             |   |          <3>|
+   *  |  1       2  |   | 1         4 |    | 1         2 |   | 2           |
+   *  LF --------- RF   LF --------- RF    LF --------- RF   LF --------- RF
+   */
+  #define LEVEL_CORNERS_LEVELING_ORDER { LF, RF, RB, LB }
 #endif
 
 /**
diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h
index 1969c98ccce..b17e81d831d 100644
--- a/Marlin/src/lcd/language/language_en.h
+++ b/Marlin/src/lcd/language/language_en.h
@@ -130,6 +130,8 @@ namespace Language_en {
   PROGMEM Language_Str MSG_LEVEL_CORNERS                   = _UxGT("Level Corners");
   PROGMEM Language_Str MSG_LEVEL_CORNERS_RAISE             = _UxGT("Raise Bed Until Probe Triggered");
   PROGMEM Language_Str MSG_LEVEL_CORNERS_IN_RANGE          = _UxGT("All Corners Within Tolerance. Level Bed");
+  PROGMEM Language_Str MSG_LEVEL_CORNERS_GOOD_POINTS       = _UxGT("Good Points: ");
+  PROGMEM Language_Str MSG_LEVEL_CORNERS_LAST_Z            = _UxGT("Last Z: ");
   PROGMEM Language_Str MSG_NEXT_CORNER                     = _UxGT("Next Corner");
   PROGMEM Language_Str MSG_MESH_EDITOR                     = _UxGT("Mesh Editor");
   PROGMEM Language_Str MSG_EDIT_MESH                       = _UxGT("Edit Mesh");
diff --git a/Marlin/src/lcd/menu/menu_bed_corners.cpp b/Marlin/src/lcd/menu/menu_bed_corners.cpp
index 2252cbc49fa..0088f306f67 100644
--- a/Marlin/src/lcd/menu/menu_bed_corners.cpp
+++ b/Marlin/src/lcd/menu/menu_bed_corners.cpp
@@ -56,6 +56,13 @@
   float last_z;
   int good_points;
   bool corner_probing_done, wait_for_probe;
+
+  #if HAS_MARLINUI_U8GLIB
+    #include "../dogm/marlinui_DOGM.h"
+  #endif
+  #define GOOD_POINTS_TO_STR(N) ui8tostr2(N)
+  #define LAST_Z_TO_STR(N) ftostr53_63(N) //ftostr42_52(N)
+
 #endif
 
 static_assert(LEVEL_CORNERS_Z_HOP >= 0, "LEVEL_CORNERS_Z_HOP must be >= 0. Please update your configuration.");
@@ -66,12 +73,89 @@ extern const char G28_STR[];
   static bool leveling_was_active = false;
 #endif
 
-static int8_t bed_corner;
+#ifndef LEVEL_CORNERS_LEVELING_ORDER
+  #define LEVEL_CORNERS_LEVELING_ORDER { LF, RF, LB, RB } // Default
+  //#define LEVEL_CORNERS_LEVELING_ORDER { LF, LB, RF  }  // 3 hard-coded points
+  //#define LEVEL_CORNERS_LEVELING_ORDER { LF, RF }       // 3-Point tramming - Rear
+  //#define LEVEL_CORNERS_LEVELING_ORDER { LF, LB }       // 3-Point tramming - Right
+  //#define LEVEL_CORNERS_LEVELING_ORDER { RF, RB }       // 3-Point tramming - Left
+  //#define LEVEL_CORNERS_LEVELING_ORDER { LB, RB }       // 3-Point tramming - Front
+#endif
 
+#define LF 1
+#define RF 2
+#define RB 3
+#define LB 4
+constexpr int lco[] = LEVEL_CORNERS_LEVELING_ORDER;
+constexpr bool level_corners_3_points = COUNT(lco) == 2;
+static_assert(level_corners_3_points || COUNT(lco) == 4, "LEVEL_CORNERS_LEVELING_ORDER must have exactly 2 or 4 corners.");
+
+constexpr int lcodiff = abs(lco[0] - lco[1]);
+static_assert(COUNT(lco) == 4 || lcodiff == 1 || lcodiff == 3, "The first two LEVEL_CORNERS_LEVELING_ORDER corners must be on the same edge.");
+
+constexpr int nr_edge_points = level_corners_3_points ? 3 : 4;
+constexpr int available_points = nr_edge_points + ENABLED(LEVEL_CENTER_TOO);
+constexpr int center_index = TERN(LEVEL_CENTER_TOO, available_points - 1, -1);
 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] };
 
+static int8_t bed_corner;
+
+/**
+ * Select next corner coordinates
+ */
+static inline void _lcd_level_bed_corners_get_next_position() {
+
+  if (level_corners_3_points) {
+    if (bed_corner >= available_points) bed_corner = 0; // Above max position -> move back to first corner
+    switch (bed_corner) {
+      case 0 ... 1:
+        // First two corners set explicitly by the configuration
+        current_position = lf;                       // Left front
+        switch (lco[bed_corner]) {
+          case RF: current_position.x = rb.x; break; // Right Front
+          case RB: current_position   = rb;   break; // Right Back
+          case LB: current_position.y = rb.y; break; // Left Back
+        }
+        break;
+
+      case 2:
+        // Determine which edge to probe for 3rd point
+        current_position.set(lf.x + (rb.x - lf.x) / 2, lf.y + (rb.y - lf.y) / 2);
+        if ((lco[0] == LB && lco[1] == RB) || (lco[0] == RB && lco[1] == LB)) current_position.y = lf.y; // Front Center
+        if ((lco[0] == LF && lco[1] == LB) || (lco[0] == LB && lco[1] == LF)) current_position.x = rb.x; // Center Right
+        if ((lco[0] == RF && lco[1] == RB) || (lco[0] == RB && lco[1] == RF)) current_position.x = lf.x; // Left Center
+        if ((lco[0] == LF && lco[1] == RF) || (lco[0] == RF && lco[1] == LF)) current_position.y = rb.y; // Center Back
+        #if DISABLED(LEVEL_CENTER_TOO) && ENABLED(LEVEL_CORNERS_USE_PROBE)
+          bed_corner++;  // Must increment the count to ensure it resets the loop if the 3rd point is out of tolerance
+        #endif
+        break;
+
+      #if ENABLED(LEVEL_CENTER_TOO)
+        case 3:
+          current_position.set(X_CENTER, Y_CENTER);
+          break;
+      #endif
+    }
+  }
+  else {
+    // Four-Corner Bed Tramming with optional center
+    if (TERN0(LEVEL_CENTER_TOO, bed_corner == center_index)) {
+      current_position.set(X_CENTER, Y_CENTER);
+      TERN_(LEVEL_CORNERS_USE_PROBE, good_points--); // Decrement to allow one additional probe point
+    }
+    else {
+      current_position = lf;                       // Left front
+      switch (lco[bed_corner]) {
+        case RF: current_position.x = rb.x; break; // Right front
+        case RB: current_position   = rb;   break; // Right rear
+        case LB: current_position.y = rb.y; break; // Left rear
+      }
+    }
+  }
+}
+
 /**
  * Level corners, starting in the front-left corner.
  */
@@ -82,8 +166,37 @@ constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1]
   VALIDATE_POINT(lf.x, Y_CENTER, "left"); VALIDATE_POINT(X_CENTER, lf.y, "front");
   VALIDATE_POINT(rb.x, Y_CENTER, "right"); VALIDATE_POINT(X_CENTER, rb.y, "back");
 
+  #ifndef PAGE_CONTAINS
+    #define PAGE_CONTAINS(...) true
+  #endif
+
   void _lcd_draw_probing() {
-    if (ui.should_draw()) MenuItem_static::draw((LCD_HEIGHT - 1) / 2, GET_TEXT(MSG_PROBING_MESH));
+    if (!ui.should_draw()) return;
+
+    TERN_(HAS_MARLINUI_U8GLIB, ui.set_font(FONT_MENU)); // Set up the font for extra info
+
+    MenuItem_static::draw(0, GET_TEXT(MSG_PROBING_MESH), SS_INVERT); // "Probing Mesh" heading
+
+    uint8_t cy = LCD_HEIGHT - 1, y = LCD_ROW_Y(cy);
+
+    // Display # of good points found vs total needed
+    if (PAGE_CONTAINS(y - (MENU_FONT_HEIGHT), y)) {
+      SETCURSOR(0, cy);
+      lcd_put_u8str_P(GET_TEXT(MSG_LEVEL_CORNERS_GOOD_POINTS));
+      lcd_put_u8str(GOOD_POINTS_TO_STR(good_points));
+      lcd_put_wchar('/');
+      lcd_put_u8str(GOOD_POINTS_TO_STR(nr_edge_points));
+    }
+
+    --cy;
+    y -= MENU_FONT_HEIGHT;
+
+    // Display the Last Z value
+    if (PAGE_CONTAINS(y - (MENU_FONT_HEIGHT), y)) {
+      SETCURSOR(0, cy);
+      lcd_put_u8str_P(GET_TEXT(MSG_LEVEL_CORNERS_LAST_Z));
+      lcd_put_u8str(LAST_Z_TO_STR(last_z));
+    }
   }
 
   void _lcd_draw_raise() {
@@ -112,7 +225,7 @@ constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1]
   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
+    do_blocking_move_to_z(last_z - LEVEL_CORNERS_PROBE_TOLERANCE, MMM_TO_MMS(Z_PROBE_SPEED_SLOW)); // 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);
@@ -149,25 +262,18 @@ constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1]
   }
 
   void _lcd_test_corners() {
-    ui.goto_screen(_lcd_draw_probing);
-    bed_corner = TERN(LEVEL_CENTER_TOO, 4, 0);
+    bed_corner = TERN(LEVEL_CENTER_TOO, center_index, 0);
     last_z = LEVEL_CORNERS_HEIGHT;
     endstops.enable_z_probe(true);
     good_points = 0;
-
+    ui.goto_screen(_lcd_draw_probing);
     do {
+      ui.refresh(LCDVIEW_REDRAW_NOW);
+      _lcd_draw_probing();                                //update screen with # of good points
       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
-      }
+
+      _lcd_level_bed_corners_get_next_position();         // Select next corner coordinates
+      current_position -= probe.offset_xy;                // Account for probe offsets
       do_blocking_move_to_xy(current_position);           // Goto corner
 
       if (!_lcd_level_bed_corners_probe()) {              // Probe down to tolerance
@@ -185,10 +291,10 @@ constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1]
           return;
       }
 
-      if (bed_corner != 4) good_points++; // ignore center
+      if (bed_corner != center_index) good_points++; // ignore center
       if (++bed_corner > 3) bed_corner = 0;
 
-    } while (good_points < 4); // loop until all corners whitin tolerance
+    } while (good_points < nr_edge_points); // loop until all points within tolerance
 
     ui.goto_screen(_lcd_draw_level_prompt); // prompt for bed leveling
     ui.set_selection(true);
@@ -198,18 +304,13 @@ constexpr xy_pos_t lf { (X_MIN_BED) + inset_lfrb[0], (Y_MIN_BED) + inset_lfrb[1]
 
   static inline void _lcd_goto_next_corner() {
     line_to_z(LEVEL_CORNERS_Z_HOP);
-    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, Y_CENTER); break;
-      #endif
-    }
+
+    // Select next corner coordinates
+    _lcd_level_bed_corners_get_next_position();
+
     line_to_current_position(manual_feedrate_mm_s.x);
     line_to_z(LEVEL_CORNERS_HEIGHT);
-    if (++bed_corner > 3 + ENABLED(LEVEL_CENTER_TOO)) bed_corner = 0;
+    if (++bed_corner >= available_points) bed_corner = 0;
   }
 
 #endif // !LEVEL_CORNERS_USE_PROBE
diff --git a/buildroot/tests/rambo-tests b/buildroot/tests/rambo-tests
index 5bc1b39f7e1..d471f4201c8 100755
--- a/buildroot/tests/rambo-tests
+++ b/buildroot/tests/rambo-tests
@@ -30,7 +30,7 @@ opt_enable REPRAP_DISCOUNT_SMART_CONTROLLER LCD_PROGRESS_BAR LCD_PROGRESS_BAR_TE
            BLINKM PCA9533 PCA9632 RGB_LED RGB_LED_R_PIN RGB_LED_G_PIN RGB_LED_B_PIN LED_CONTROL_MENU \
            NEOPIXEL_LED CASE_LIGHT_ENABLE CASE_LIGHT_USE_NEOPIXEL CASE_LIGHT_MENU \
            PID_PARAMS_PER_HOTEND PID_AUTOTUNE_MENU PID_EDIT_MENU LCD_SHOW_E_TOTAL \
-           PRINTCOUNTER SERVICE_NAME_1 SERVICE_INTERVAL_1 LEVEL_BED_CORNERS \
+           PRINTCOUNTER SERVICE_NAME_1 SERVICE_INTERVAL_1 LEVEL_BED_CORNERS LEVEL_CENTER_TOO \
            NOZZLE_PARK_FEATURE FILAMENT_RUNOUT_SENSOR FILAMENT_RUNOUT_DISTANCE_MM \
            ADVANCED_PAUSE_FEATURE FILAMENT_LOAD_UNLOAD_GCODES FILAMENT_UNLOAD_ALL_EXTRUDERS \
            PASSWORD_FEATURE PASSWORD_ON_STARTUP PASSWORD_ON_SD_PRINT_MENU PASSWORD_AFTER_SD_PRINT_END PASSWORD_AFTER_SD_PRINT_ABORT \
@@ -53,10 +53,12 @@ opt_set EXTRUDERS 0
 opt_set TEMP_SENSOR_0 999
 opt_set DUMMY_THERMISTOR_999_VALUE 170
 opt_set DIGIPOT_MOTOR_CURRENT '{ 120, 120, 120, 120, 120 }'
+opt_set LEVEL_CORNERS_LEVELING_ORDER '{ LF, RF }'
 opt_enable USE_XMAX_PLUG USE_YMAX_PLUG USE_ZMAX_PLUG \
            REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER REVERSE_ENCODER_DIRECTION SDSUPPORT EEPROM_SETTINGS \
            S_CURVE_ACCELERATION X_DUAL_STEPPER_DRIVERS X_DUAL_ENDSTOPS Y_DUAL_STEPPER_DRIVERS Y_DUAL_ENDSTOPS \
-           ADAPTIVE_STEP_SMOOTHING CNC_COORDINATE_SYSTEMS GCODE_MOTION_MODES
+           ADAPTIVE_STEP_SMOOTHING CNC_COORDINATE_SYSTEMS GCODE_MOTION_MODES \
+           LEVEL_BED_CORNERS LEVEL_CENTER_TOO
 opt_disable MIN_SOFTWARE_ENDSTOP_Z MAX_SOFTWARE_ENDSTOPS
 exec_test $1 $2 "Rambo CNC Configuration" "$3"