Back to the differences method in run_z_probe()

AnHardt · Jul 21, 2016 · 6ef26a8 · 6ef26a8
1 parent 16e5348
commit 6ef26a8
Showing 1 changed file with 17 additions and 3 deletions.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
@@ -2064,6 +2064,12 @@ static void clean_up_after_endstop_or_probe_move() {
     return false;
   }
 
+  #if ENABLED(DELTA)
+    #define SET_CURRENT_FROM_STEPPERS() current_position[Z_AXIS] = z_before - stepper.get_axis_position_mm(Z_AXIS) + z_mm
+  #else
+    #define SET_CURRENT_FROM_STEPPERS() current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS)
+  #endif
+
   // Do a single Z probe and return with current_position[Z_AXIS]
   // at the height where the probe triggered.
   static float run_z_probe() {
@@ -2075,23 +2081,31 @@ static void clean_up_after_endstop_or_probe_move() {
       planner.bed_level_matrix.set_to_identity();
     #endif
 
+    #if ENABLED(DELTA)
+      float z_before = current_position[Z_AXIS];
+      float z_mm = stepper.get_axis_position_mm(Z_AXIS);
+    #endif
     current_position[Z_AXIS] = -(Z_MAX_LENGTH + 10);
     do_blocking_move_to_z(current_position[Z_AXIS], Z_PROBE_SPEED_FAST);
     endstops.hit_on_purpose(); // clear endstop hit flags
     // Get the current stepper position after bumping an endstop
-    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
-    SYNC_PLAN_POSITION_KINEMATIC(); // tell the planner where we are      feedrate_mm_m = homing_feedrate_mm_m[Z_AXIS];
+    SET_CURRENT_FROM_STEPPERS();
+    SYNC_PLAN_POSITION_KINEMATIC(); // tell the planner where we are
 
     // move up the retract distance
     current_position[Z_AXIS] += home_bump_mm(Z_AXIS);
     do_blocking_move_to_z(current_position[Z_AXIS], Z_PROBE_SPEED_FAST);
 
+    #if ENABLED(DELTA)
+      z_before = current_position[Z_AXIS];
+      z_mm = stepper.get_axis_position_mm(Z_AXIS);
+    #endif
     // move back down slowly to find bed
     current_position[Z_AXIS] -= home_bump_mm(Z_AXIS) * 2;
     do_blocking_move_to_z(current_position[Z_AXIS], Z_PROBE_SPEED_SLOW);
     endstops.hit_on_purpose(); // clear endstop hit flags
     // Get the current stepper position after bumping an endstop
-    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+    SET_CURRENT_FROM_STEPPERS();
     SYNC_PLAN_POSITION_KINEMATIC(); // tell the planner where we are
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)