Merge pull request #4361 from AnHardt/imp-dbmt-for-delta

DELTA do_blocking_move_to() more like the Chartesian one

Marlin/Marlin_main.cpp

@@ -1679,15 +1679,37 @@ 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;
 
-    destination[X_AXIS] = x;
+    // when in the danger zone
+    if (current_position[Z_AXIS] > delta_clip_start_height) {
+      if (delta_clip_start_height < z) { // staying in the danger zone
+        destination[X_AXIS] = x;         // move directly
         destination[Y_AXIS] = y;
         destination[Z_AXIS] = z;
-
-    if (x == current_position[X_AXIS] && y == current_position[Y_AXIS])
         prepare_move_to_destination_raw(); // this will also set_current_to_destination
-    else
+        return;
+      } else {                           // leave the danger zone
+        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_raw(); // 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_raw(); // this will also set_current_to_destination
+    }
+    destination[X_AXIS] = x;
+    destination[Y_AXIS] = y;
+    destination[Z_AXIS] = current_position[Z_AXIS];
     prepare_move_to_destination(); // this will also set_current_to_destination
 
+    if (current_position[Z_AXIS] > z) { // lower
+      destination[Z_AXIS] = z;
+      prepare_move_to_destination_raw(); // this will also set_current_to_destination
+    }
+
   #else
 
     // If Z needs to raise, do it before moving XY