From 6803c8e37eab3dd71a8db30defe5787c90372a92 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 23 Jun 2016 15:50:13 -0700
Subject: [PATCH] Move Z up before, or down after XY, for "blocking move"

---
 Marlin/Marlin_main.cpp | 19 +++++++++++++------
 1 file changed, 13 insertions(+), 6 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0e42a4bad0b..369e799774f 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1666,18 +1666,25 @@ static void clean_up_after_endstop_or_probe_move() {
 
     #else
 
-      feedrate = homing_feedrate[Z_AXIS];
-
-      current_position[Z_AXIS] = z;
-      line_to_current_position();
-      stepper.synchronize();
+      // If Z needs to raise, do it before moving XY
+      if (current_position[Z_AXIS] < z) {
+        feedrate = homing_feedrate[Z_AXIS];
+        current_position[Z_AXIS] = z;
+        line_to_current_position();
+      }
 
       feedrate = XY_PROBE_FEEDRATE;
-
       current_position[X_AXIS] = x;
       current_position[Y_AXIS] = y;
       line_to_current_position();
 
+      // If Z needs to lower, do it after moving XY
+      if (current_position[Z_AXIS] > z) {
+        feedrate = homing_feedrate[Z_AXIS];
+        current_position[Z_AXIS] = z;
+        line_to_current_position();
+      }
+
     #endif
 
     stepper.synchronize();