diff --git a/Marlin/src/gcode/bedlevel/mbl/G29.cpp b/Marlin/src/gcode/bedlevel/mbl/G29.cpp index 0f2889a55c..de86edfdee 100644 --- a/Marlin/src/gcode/bedlevel/mbl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/mbl/G29.cpp @@ -104,6 +104,8 @@ void GcodeSuite::G29() { // 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. @@ -114,22 +116,14 @@ void GcodeSuite::G29() { } // 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