DELTA do_blocking_move_to() more like the Chartesian one

Author: AnHardt

Marlin/Marlin_main.cpp

@@ -1678,14 +1678,28 @@ void do_blocking_move_to(float x, float y, float z, float fr_mm_m /*=0.0*/) {
 
     feedrate_mm_m = (fr_mm_m != 0.0) ? fr_mm_m : XY_PROBE_FEEDRATE_MM_M;
 
+    // when in the danger zone and target is below the danger zone firs move out
+    if ((current_position[Z_AXIS] > delta_clip_start_height) && (delta_clip_start_height < z)) {
+      destination[X_AXIS] = current_position[X_AXIS];
+      destination[Y_AXIS] = current_position[Y_AXIS];
+      destination[Z_AXIS] = delta_clip_start_height;
+      prepare_move_to_destination();     // this will also set_current_to_destination
+    }
+    if (current_position[Z_AXIS] < z) {  // raise
+      destination[X_AXIS] = current_position[X_AXIS];
+      destination[Y_AXIS] = current_position[Y_AXIS];
+      destination[Z_AXIS] = z;
+      prepare_move_to_destination();     // this will also set_current_to_destination
+    }
     destination[X_AXIS] = x;
     destination[Y_AXIS] = y;
-    destination[Z_AXIS] = z;
+    destination[Z_AXIS] = current_position[Z_AXIS];
+    prepare_move_to_destination_raw(); // this will also set_current_to_destination
 
-    if (x == current_position[X_AXIS] && y == current_position[Y_AXIS])
-      prepare_move_to_destination_raw(); // this will also set_current_to_destination
-    else
+    if (current_position[Z_AXIS] > z) { // lower
+      destination[Z_AXIS] = z;
       prepare_move_to_destination();     // this will also set_current_to_destination
+    }
 
   #else
 
@@ -2079,7 +2093,7 @@ static void clean_up_after_endstop_or_probe_move() {
     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];
+    SYNC_PLAN_POSITION_KINEMATIC(); // tell the planner where we are
 
     // move up the retract distance
     current_position[Z_AXIS] += home_bump_mm(Z_AXIS);