mcu: Pass delta velocity and acceleration directly to mcu_stepper

Rework the parameters of step_delta_const() and step_delta_accel() so
that they take velocity and acceleration directly in millimeters and
seconds.  This simplifies the delta.py kinematic code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 47 insertions, 55 deletions

diff --git a/klippy/delta.py b/klippy/delta.py
index 009fb7b6..9722ff3d 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -183,19 +183,13 @@ class DeltaKinematics:
 
         origx, origy, origz = move.start_pos[:3]
 
+        accel = move.accel
+        cruise_v = move.cruise_v
         accel_t = move.accel_t
         cruise_end_t = accel_t + move.cruise_t
         accel_d = move.accel_r * move_d
         cruise_end_d = accel_d + move.cruise_r * move_d
 
-        inv_cruise_v = 1. / move.cruise_v
-        inv_accel = 1. / move.accel
-        accel_time_offset = move.start_v * inv_accel
-        accel_multiplier = 2.0 * inv_accel
-        accel_offset = move.start_v**2 * 0.5 * inv_accel
-        decel_time_offset = move.cruise_v * inv_accel + cruise_end_t
-        decel_offset = move.cruise_v**2 * 0.5 * inv_accel + cruise_end_d
-
         for i in StepList:
             # Find point on line of movement closest to tower
             towerx_d = self.towers[i][0] - origx
@@ -235,44 +229,32 @@ class DeltaKinematics:
             # Generate steps
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
-            step_pos = mcu_stepper.commanded_position
-            step_dist = self.steppers[i].step_dist
-            height = step_pos*step_dist - origz
             if accel_up_d > 0.:
-                count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, accel_up_d,
-                    accel_offset, accel_multiplier, step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
-                height += count * step_dist
+                mcu_stepper.step_delta_accel(
+                    mcu_time, 0., accel_up_d, move.start_v, accel,
+                    origz, closestxy_d, closest_height2, movez_r)
             if cruise_up_d > 0.:
-                count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, cruise_up_d,
-                    -accel_d, inv_cruise_v, step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
-                height += count * step_dist
+                mcu_stepper.step_delta_const(
+                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v,
+                    origz, closestxy_d, closest_height2, movez_r)
             if decel_up_d > 0.:
-                count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, decel_up_d,
-                    -decel_offset, -accel_multiplier, step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
-                height += count * step_dist
+                mcu_stepper.step_delta_accel(
+                    mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
+                    cruise_v, -accel,
+                    origz, closestxy_d, closest_height2, movez_r)
             if accel_down_d > 0.:
-                count = mcu_stepper.step_delta_accel(
-                    mcu_time - accel_time_offset, accel_down_d,
-                    accel_offset, accel_multiplier, -step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
-                height += count * step_dist
+                mcu_stepper.step_delta_accel(
+                    mcu_time, 0., -accel_down_d, move.start_v, accel,
+                    origz, closestxy_d, closest_height2, movez_r)
             if cruise_down_d > 0.:
-                count = mcu_stepper.step_delta_const(
-                    mcu_time + accel_t, cruise_down_d,
-                    -accel_d, inv_cruise_v, -step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
-                height += count * step_dist
+                mcu_stepper.step_delta_const(
+                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v,
+                    origz, closestxy_d, closest_height2, movez_r)
             if decel_down_d > 0.:
-                count = mcu_stepper.step_delta_accel(
-                    mcu_time + decel_time_offset, decel_down_d,
-                    -decel_offset, -accel_multiplier, -step_dist,
-                    height, closestxy_d, closest_height2, movez_r)
+                mcu_stepper.step_delta_accel(
+                    mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
+                    cruise_v, -accel,
+                    origz, closestxy_d, closest_height2, movez_r)
 
 
 ######################################################################
diff --git a/klippy/mcu.py b/klippy/mcu.py
index be04308b..5e51b14d 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -139,31 +139,41 @@ class MCU_stepper:
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self.commanded_position += count
-    def step_delta_const(self, mcu_time, dist, start_pos
-                         , inv_velocity, step_dist
-                         , height, closestxy_d, closest_height2, movez_r):
-        clock = mcu_time * self._mcu_freq
+    def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
+                         , height_base, closestxy_d, closest_height2, movez_r):
+        mcu_freq = self._mcu_freq
+        step_dist = self._step_dist
+        height = self.commanded_position*step_dist - height_base
+        if dist < 0:
+            dist = -dist
+            step_dist = -step_dist
         count = self._ffi_lib.stepcompress_push_delta_const(
-            self._stepqueue, clock, dist, start_pos
-            , inv_velocity * self._mcu_freq, step_dist
+            self._stepqueue, mcu_time * mcu_freq, dist, -start_pos
+            , mcu_freq / cruise_v, step_dist
             , height, closestxy_d, closest_height2, movez_r)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self.commanded_position += count
-        return count
-    def step_delta_accel(self, mcu_time, dist, start_pos
-                         , accel_multiplier, step_dist
-                         , height, closestxy_d, closest_height2, movez_r):
-        clock = mcu_time * self._mcu_freq
-        mcu_freq2 = self._mcu_freq**2
+    def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel
+                         , height_base, closestxy_d, closest_height2, movez_r):
+        inv_step_dist = self._inv_step_dist
+        mcu_freq = self._mcu_freq
+        inv_accel = 1. / accel
+        time_offset = start_v * inv_accel * mcu_freq
+        accel_offset = start_v**2 * 0.5 * inv_accel
+        step_dist = self._step_dist
+        height = self.commanded_position*step_dist - height_base
+        if dist < 0:
+            dist = -dist
+            step_dist = -step_dist
+        clock = mcu_time * mcu_freq - time_offset
         count = self._ffi_lib.stepcompress_push_delta_accel(
-            self._stepqueue, clock, dist, start_pos
-            , accel_multiplier * mcu_freq2, step_dist
+            self._stepqueue, clock, dist, accel_offset - start_pos
+            , 2. * inv_accel * mcu_freq**2, step_dist
             , height, closestxy_d, closest_height2, movez_r)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self.commanded_position += count
-        return count
 
 class MCU_endstop:
     error = error