From 96b71e62c0604e3ae60660fcae31e9a3bc5352ac Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sun, 8 May 2016 18:28:46 -0700
Subject: [PATCH] Fix dipping on DELTA robots during G29

- Addressing #3689, et. al.
---
 Marlin/Marlin_main.cpp | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 5c9e5ff5448..830709595c1 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -343,6 +343,7 @@ static uint8_t target_extruder;
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
   int xy_travel_speed = XY_TRAVEL_SPEED;
   float zprobe_zoffset = Z_PROBE_OFFSET_FROM_EXTRUDER;
+  bool bed_leveling_in_progress = false;
 #endif
 
 #if ENABLED(Z_DUAL_ENDSTOPS) && DISABLED(DELTA)
@@ -1632,7 +1633,12 @@ static void setup_for_endstop_move() {
       destination[X_AXIS] = x;
       destination[Y_AXIS] = y;
       destination[Z_AXIS] = z;
-      prepare_move_raw(); // this will also set_current_to_destination
+
+      if (x == current_position[X_AXIS] && y == current_position[Y_AXIS])
+        prepare_move_raw(); // this will also set_current_to_destination
+      else
+        prepare_move();     // this will also set_current_to_destination
+
       stepper.synchronize();
 
     #else
@@ -3238,6 +3244,8 @@ inline void gcode_G28() {
 
     feedrate = homing_feedrate[Z_AXIS];
 
+    bed_leveling_in_progress = true;
+
     #if ENABLED(AUTO_BED_LEVELING_GRID)
 
       // probe at the points of a lattice grid
@@ -3585,15 +3593,17 @@ inline void gcode_G28() {
       stepper.synchronize();
     #endif
 
-    KEEPALIVE_STATE(IN_HANDLER);
-
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) {
         SERIAL_ECHOLNPGM("<<< gcode_G29");
       }
     #endif
 
+    bed_leveling_in_progress = false;
+
     report_current_position();
+
+    KEEPALIVE_STATE(IN_HANDLER);
   }
 
   #if DISABLED(Z_PROBE_SLED) // could be avoided
@@ -7305,7 +7315,7 @@ void mesh_buffer_line(float x, float y, float z, const float e, float feed_rate,
       calculate_delta(target);
 
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-        adjust_delta(target);
+        if (!bed_leveling_in_progress) adjust_delta(target);
       #endif
 
       //DEBUG_POS("prepare_move_delta", target);