diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 84b7cd1071..26019885c6 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -4258,6 +4258,8 @@ void home_all_axes() { gcode_G28(true); } // For the initial G29 S2 save software endstop state enable_soft_endstops = soft_endstops_enabled; #endif + // Move close to the bed before the first point + do_blocking_move_to_z(Z_MIN_POS); } else { // For G29 S2 after adjusting Z. @@ -4268,22 +4270,14 @@ void home_all_axes() { gcode_G28(true); } } // If there's another point to sample, move there with optional lift. if (mbl_probe_index < GRID_MAX_POINTS) { - mbl.zigzag(mbl_probe_index, px, py); - _manual_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]); - #if HAS_SOFTWARE_ENDSTOPS // Disable software endstops to allow manual adjustment // If G29 is not completed, they will not be re-enabled soft_endstops_enabled = false; #endif - // Move close to the bed for the first point - if (!mbl_probe_index) { - current_position[Z_AXIS] = Z_MIN_POS; - buffer_line_to_current_position(); - } - - mbl_probe_index++; + mbl.zigzag(mbl_probe_index++, px, py); + _manual_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]); } else { // One last "return to the bed" (as originally coded) at completion