From 8e31640229e6b147550661c76c7986714c421e35 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 15 Sep 2016 15:38:48 -0500
Subject: [PATCH] Optimize prepare_kinematic_move_to

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

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 9f5965300..e61ca183f 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -8043,28 +8043,59 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
    * small incremental moves for DELTA or SCARA.
    */
   inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) {
+
+    // Get the top feedrate of the move in the XY plane
+    float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
+
+    // If the move is only in Z don't split up the move.
+    // This shortcut cannot be used if planar bed leveling
+    // is in use, but is fine with mesh-based bed leveling
+    if (logical[X_AXIS] == current_position[X_AXIS] && logical[Y_AXIS] == current_position[Y_AXIS]) {
+      inverse_kinematics(logical);
+      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
+      return true;
+    }
+
+    // Get the distance moved in XYZ
     float difference[NUM_AXIS];
     LOOP_XYZE(i) difference[i] = logical[i] - current_position[i];
 
     float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
     if (UNEAR_ZERO(cartesian_mm)) return false;
-    float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
+
+    // Minimum number of seconds to move the given distance
     float seconds = cartesian_mm / _feedrate_mm_s;
-    int steps = max(1, int(delta_segments_per_second * seconds));
-    float inv_steps = 1.0/steps;
 
-    // SERIAL_ECHOPAIR("mm=", cartesian_mm);
-    // SERIAL_ECHOPAIR(" seconds=", seconds);
-    // SERIAL_ECHOLNPAIR(" steps=", steps);
+    // The number of segments-per-second times the duration
+    // gives the number of segments we should produce
+    uint16_t segments = delta_segments_per_second * seconds;
 
-    for (int s = 1; s <= steps; s++) {
+    #if IS_SCARA
+      NOMORE(segments, cartesian_mm * 2);
+    #endif
 
-      float fraction = float(s) * inv_steps;
+    NOLESS(segments, 1);
+
+    // Each segment produces this much of the move
+    float inv_segments = 1.0 / segments,
+          segment_distance[XYZE] = {
+            difference[X_AXIS] * inv_segments,
+            difference[Y_AXIS] * inv_segments,
+            difference[Z_AXIS] * inv_segments,
+            difference[E_AXIS] * inv_segments
+          };
+
+    // SERIAL_ECHOPAIR("mm=", cartesian_mm);
+    // SERIAL_ECHOPAIR(" seconds=", seconds);
+    // SERIAL_ECHOLNPAIR(" segments=", segments);
 
-      LOOP_XYZE(i)
-        logical[i] = current_position[i] + difference[i] * fraction;
+    // Set the target to the current position to start
+    LOOP_XYZE(i) logical[i] = current_position[i];
 
+    // Send all the segments to the planner
+    for (uint16_t s = 0; s < segments; s++) {
+      LOOP_XYZE(i) logical[i] += segment_distance[i];
       inverse_kinematics(logical);
 
       //DEBUG_POS("prepare_kinematic_move_to", logical);