From 53f0c7522a127abe7bb881e3d6e2104d8141bff9 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Thu, 29 Mar 2018 18:42:31 -0500
Subject: [PATCH] Symmetrical FWRETRACT Z Hop

Do the Z lift normally before setting Z back to its prior value. But do the Z lower using spoofing. This should produce proper symmetrical movement.
---
 Marlin/src/feature/fwretract.cpp | 23 ++++++++++++-----------
 1 file changed, 12 insertions(+), 11 deletions(-)

diff --git a/Marlin/src/feature/fwretract.cpp b/Marlin/src/feature/fwretract.cpp
index 3c75260844..15567f7a2d 100644
--- a/Marlin/src/feature/fwretract.cpp
+++ b/Marlin/src/feature/fwretract.cpp
@@ -125,7 +125,6 @@ void FWRetract::retract(const bool retracting
     SERIAL_ECHOLNPAIR("hop_amount ", hop_amount);
   //*/
 
-  const bool has_zhop = retract_zlift > 0.01;     // Is there a hop set?
   const float old_feedrate_mm_s = feedrate_mm_s;
 
   // The current position will be the destination for E and Z moves
@@ -144,23 +143,25 @@ void FWRetract::retract(const bool retracting
     // Is a Z hop set, and has the hop not yet been done?
     // No double zlifting
     // Feedrate to the max
-    if (has_zhop && !hop_amount) {
-      hop_amount += retract_zlift;                        // Carriage is raised for retraction hop
-      feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];  // Z feedrate to max
-      current_position[Z_AXIS] -= retract_zlift;          // Pretend current pos is lower. Next move raises Z.
-      SYNC_PLAN_POSITION_KINEMATIC();                     // Set the planner to the new position
+    if (retract_zlift > 0.01 && !hop_amount) {            // Apply hop only once
+      const float old_z = current_position[Z_AXIS];
+      hop_amount += retract_zlift;                        // Add to the hop total (again, only once)
+      destination[Z_AXIS] += retract_zlift;               // Raise Z by the zlift (M207 Z) amount
+      feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];  // Maximum Z feedrate
       prepare_move_to_destination();                      // Raise up to the old current pos
+      current_position[Z_AXIS] = old_z;                   // Spoof the Z position
+      SYNC_PLAN_POSITION_KINEMATIC();
     }
   }
   else {
     // If a hop was done and Z hasn't changed, undo the Z hop
     if (hop_amount) {
-      current_position[Z_AXIS] += retract_zlift;          // Pretend current pos is higher. Next move lowers Z.
-      SYNC_PLAN_POSITION_KINEMATIC();                     // Set the planner to the new position
+      current_position[Z_AXIS] += hop_amount;             // Set actual Z (due to the prior hop)
       feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS];  // Z feedrate to max
-      prepare_move_to_destination();                      // Lower down to the old current pos
-      hop_amount = 0.0;                                   // Clear hop
-    }
+      prepare_move_to_destination();                      // Lower Z and update current_position
+      SYNC_PLAN_POSITION_KINEMATIC();                     // Update the planner
+      hop_amount = 0.0;                                   // Clear the hop amount
+   }
 
     // A retract multiplier has been added here to get faster swap recovery
     feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s;