Tweak MBL move to bed code

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