From bb93b6822a0eb86acab8fcf97ae4eeacf3b0e4d4 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sat, 23 Jul 2016 13:11:18 -0700
Subject: [PATCH] Use set_current_from_steppers_for_axis

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b1cfad4bbb..b92a222425 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -564,7 +564,7 @@ void stop();
 void get_available_commands();
 void process_next_command();
 void prepare_move_to_destination();
-void set_current_from_steppers();
+void set_current_from_steppers_for_axis(AxisEnum axis);
 
 #if ENABLED(ARC_SUPPORT)
   void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
@@ -2116,7 +2116,7 @@ static void clean_up_after_endstop_or_probe_move() {
 
     do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST);
     endstops.hit_on_purpose();
-    set_current_from_steppers();
+    set_current_from_steppers_for_axis(Z_AXIS);
     SYNC_PLAN_POSITION_KINEMATIC();
 
     // move up the retract distance
@@ -2125,7 +2125,7 @@ static void clean_up_after_endstop_or_probe_move() {
     // move back down slowly to find bed
     do_blocking_move_to_z(current_position[Z_AXIS] - home_bump_mm(Z_AXIS) * 2, Z_PROBE_SPEED_SLOW);
     endstops.hit_on_purpose();
-    set_current_from_steppers();
+    set_current_from_steppers_for_axis(Z_AXIS);
     SYNC_PLAN_POSITION_KINEMATIC();
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -6074,8 +6074,8 @@ void quickstop_stepper() {
   stepper.quick_stop();
   #if DISABLED(SCARA)
     stepper.synchronize();
-    set_current_from_steppers();
-    sync_plan_position();                       // ...re-apply to planner position
+    LOOP_XYZ(i) set_current_from_steppers_for_axis((AxisEnum)i);
+    SYNC_PLAN_POSITION_KINEMATIC();
   #endif
 }
 
@@ -7912,25 +7912,16 @@ void clamp_to_software_endstops(float target[3]) {
 
 #endif // DELTA
 
-void set_current_from_steppers() {
+void set_current_from_steppers_for_axis(AxisEnum axis) {
   #if ENABLED(DELTA)
     set_cartesian_from_steppers();
-    current_position[X_AXIS] = cartesian_position[X_AXIS];
-    current_position[Y_AXIS] = cartesian_position[Y_AXIS];
-    current_position[Z_AXIS] = cartesian_position[Z_AXIS];
+    current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
   #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
-    vector_3 pos = planner.adjusted_position(); // values directly from steppers...
-    current_position[X_AXIS] = pos.x;
-    current_position[Y_AXIS] = pos.y;
-    current_position[Z_AXIS] = pos.z;
+    vector_3 pos = planner.adjusted_position();
+    current_position[axis] = LOGICAL_POSITION(axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z, axis);
   #else
-    current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
-    current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
-    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+    current_position[axis] = LOGICAL_POSITION(stepper.get_axis_position_mm(axis), axis); // CORE handled transparently
   #endif
-
-  for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
-    current_position[i] += LOGICAL_POSITION(0, i);
 }
 
 #if ENABLED(MESH_BED_LEVELING)