Simplify MBL movement, zigzag

Marlin/Marlin_main.cpp

@@ -2799,6 +2799,28 @@ inline void gcode_G28() {
 
   enum MeshLevelingState { MeshReport, MeshStart, MeshNext, MeshSet, MeshSetZOffset };
 
+  inline void _mbl_goto_xy(float x, float y) {
+    saved_feedrate = feedrate;
+    feedrate = homing_feedrate[X_AXIS];
+
+    #if MIN_Z_HEIGHT_FOR_HOMING > 0
+      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + MIN_Z_HEIGHT_FOR_HOMING;
+      line_to_current_position();
+    #endif
+
+    current_position[X_AXIS] = x + home_offset[X_AXIS];
+    current_position[Y_AXIS] = y + home_offset[Y_AXIS];
+    line_to_current_position();
+
+    #if MIN_Z_HEIGHT_FOR_HOMING > 0
+      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+      line_to_current_position();
+    #endif
+
+    feedrate = saved_feedrate;
+    st_synchronize();
+  }
+
   /**
    * G29: Mesh-based Z probe, probes a grid and produces a
    *      mesh to compensate for variable bed height
@@ -2866,33 +2888,28 @@ inline void gcode_G28() {
           SERIAL_PROTOCOLLNPGM("Start mesh probing with \"G29 S1\" first.");
           return;
         }
+        // For each G29 S2...
         if (probe_point == 0) {
-          // Set Z to a positive value before recording the first Z.
-          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
+          // For the intial G29 S2 make Z a positive value (e.g., 4.0)
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
           sync_plan_position();
         }
         else {
-          // For others, save the Z of the previous point, then raise Z again.
-          ix = (probe_point - 1) % (MESH_NUM_X_POINTS);
-          iy = (probe_point - 1) / (MESH_NUM_X_POINTS);
-          if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-          mbl.set_z(ix, iy, current_position[Z_AXIS]);
-          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS];
-          plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
-          st_synchronize();
+          // For G29 S2 after adjusting Z.
+          mbl.set_zigzag_z(probe_point - 1, current_position[Z_AXIS]);
         }
-        // Is there another point to sample? Move there.
+        // If there's another point to sample, move there with optional lift.
         if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
-          ix = probe_point % (MESH_NUM_X_POINTS);
-          iy = probe_point / (MESH_NUM_X_POINTS);
-          if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-          current_position[X_AXIS] = mbl.get_x(ix) + home_offset[X_AXIS];
-          current_position[Y_AXIS] = mbl.get_y(iy) + home_offset[Y_AXIS];
-          plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS] / 60, active_extruder);
-          st_synchronize();
+          mbl.zigzag(probe_point, ix, iy);
+          _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
           probe_point++;
         }
         else {
+          // One last "return to the bed" (as originally coded) at completion
+          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+          line_to_current_position();
+          st_synchronize();
+
           // After recording the last point, activate the mbl and home
           SERIAL_PROTOCOLLNPGM("Mesh probing done.");
           probe_point = -1;

Marlin/mesh_bed_leveling.h

@@ -41,6 +41,18 @@
     float get_y(int i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
     void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
 
+    inline void zigzag(int index, int &ix, int &iy) {
+      ix = index % (MESH_NUM_X_POINTS);
+      iy = index / (MESH_NUM_X_POINTS);
+      if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
+    }
+
+    void set_zigzag_z(int index, float z) {
+      int ix, iy;
+      zigzag(index, ix, iy);
+      set_z(ix, iy, z);
+    }
+
     int select_x_index(float x) {
       int i = 1;
       while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++;

Marlin/ultralcd.cpp

@@ -887,6 +887,28 @@ void lcd_cooldown() {
    */
 
   static int _lcd_level_bed_position;
+  static bool mbl_wait_for_move = false;
+
+  // Utility to go to the next mesh point
+  // A raise is added between points if MIN_Z_HEIGHT_FOR_HOMING is in use
+  // Note: During Manual Bed Leveling the homed Z position is MESH_HOME_SEARCH_Z
+  // Z position will be restored with the final action, a G28
+  inline void _mbl_goto_xy(float x, float y) {
+    mbl_wait_for_move = true;
+    #if MIN_Z_HEIGHT_FOR_HOMING > 0
+      current_position[Z_AXIS] += MIN_Z_HEIGHT_FOR_HOMING;
+      line_to_current(Z_AXIS);
+    #endif
+    current_position[X_AXIS] = x + home_offset[X_AXIS];
+    current_position[Y_AXIS] = y + home_offset[Y_AXIS];
+    line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+    #if MIN_Z_HEIGHT_FOR_HOMING > 0
+      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
+      line_to_current(Z_AXIS);
+    #endif
+    st_synchronize();
+    mbl_wait_for_move = false;
+  }
 
   /**
    * 5. MBL Wait for controller movement and clicks:
@@ -894,7 +916,6 @@ void lcd_cooldown() {
    *        - Click saves the Z, goes to the next mesh point
    */
   static void _lcd_level_bed_procedure() {
-    static bool mbl_wait_for_move = false;
     // Menu handlers may be called in a re-entrant fashion
     // if they call st_synchronize or plan_buffer_line. So
     // while waiting for a move we just ignore new input.
@@ -931,11 +952,7 @@ void lcd_cooldown() {
     if (LCD_CLICKED) {
       if (!debounce_click) {
         debounce_click = true; // ignore multiple "clicks" in a row
-        int ix = _lcd_level_bed_position % (MESH_NUM_X_POINTS),
-            iy = _lcd_level_bed_position / (MESH_NUM_X_POINTS);
-        if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
-        mbl.set_z(ix, iy, current_position[Z_AXIS]);
-        _lcd_level_bed_position++;
+        mbl.set_zigzag_z(_lcd_level_bed_position++, current_position[Z_AXIS]);
         if (_lcd_level_bed_position == (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
           lcd_return_to_status();
           LCD_ALERTMESSAGEPGM(MSG_LEVEL_BED_DONE);
@@ -953,17 +970,9 @@ void lcd_cooldown() {
           #if ENABLED(NEWPANEL)
             lcd_quick_feedback();
           #endif
-          mbl_wait_for_move = true;
-          current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
-          line_to_current(Z_AXIS);
-          ix = _lcd_level_bed_position % (MESH_NUM_X_POINTS);
-          iy = _lcd_level_bed_position / (MESH_NUM_X_POINTS);
-          if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
-          current_position[X_AXIS] = mbl.get_x(ix);
-          current_position[Y_AXIS] = mbl.get_y(iy);
-          line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
-          st_synchronize();
-          mbl_wait_for_move = false;
+          int ix, iy;
+          mbl.zigzag(_lcd_level_bed_position, ix, iy);
+          _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
           encoderPosition = 0;
         }
       }
@@ -980,12 +989,11 @@ void lcd_cooldown() {
   static void _lcd_level_bed_homing_done() {
     if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_LEVEL_BED_WAITING), NULL);
     lcdDrawUpdate = LCDVIEW_CALL_NO_REDRAW;
+    if (mbl_wait_for_move) return;
     if (LCD_CLICKED) {
       current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-      current_position[X_AXIS] = MESH_MIN_X;
-      current_position[Y_AXIS] = MESH_MIN_Y;
-      line_to_current(manual_feedrate[X_AXIS] <= manual_feedrate[Y_AXIS] ? X_AXIS : Y_AXIS);
+      _mbl_goto_xy(MESH_MIN_X, MESH_MIN_Y);
       _lcd_level_bed_position = 0;
       lcd_goto_menu(_lcd_level_bed_procedure, true);
     }