mcu: Pass constant velocity and acceleration directly to mcu_stepper

Rename step_sqrt/step_factor to step_accel/step_const and have them
directly take the velocity and acceleration in millimeters and
seconds.  This simplifies the kinematic classes.

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

1 files changed, 28 insertions, 12 deletions

diff --git a/klippy/mcu.py b/klippy/mcu.py
index f5d7489d..be04308b 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -28,6 +28,8 @@ class MCU_stepper:
         self._step_pin, pullup, self._invert_step = parse_pin_extras(step_pin)
         self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
         self.commanded_position = 0
+        self._step_dist = self._inv_step_dist = 1.
+        self._velocity_factor = self._inv_accel_factor = 0.
         self._mcu_position_offset = 0
         self._mcu_freq = self._min_stop_interval = 0.
         self._reset_cmd = self._get_position_cmd = None
@@ -35,8 +37,13 @@ class MCU_stepper:
         self.print_to_mcu_time = mcu.print_to_mcu_time
     def set_min_stop_interval(self, min_stop_interval):
         self._min_stop_interval = min_stop_interval
+    def set_step_distance(self, step_dist):
+        self._step_dist = step_dist
+        self._inv_step_dist = 1. / step_dist
     def build_config(self):
         self._mcu_freq = self._mcu.get_mcu_freq()
+        self._velocity_factor = 1. / (self._mcu_freq * self._step_dist)
+        self._inv_accel_factor = self._mcu_freq**2 * self._step_dist
         max_error = self._mcu.get_max_stepper_error()
         min_stop_interval = max(0., self._min_stop_interval - max_error)
         self._mcu.add_config_cmd(
@@ -105,24 +112,33 @@ class MCU_stepper:
             self.commanded_position += 1
         else:
             self.commanded_position -= 1
-    def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
-        clock = mcu_time * self._mcu_freq
-        mcu_freq2 = self._mcu_freq**2
-        count = self._ffi_lib.stepcompress_push_sqrt(
-            self._stepqueue, steps, step_offset, clock
-            , sqrt_offset * mcu_freq2, factor * mcu_freq2)
+    def step_const(self, mcu_time, start_pos, dist, cruise_v):
+        #t = pos/cruise_v
+        inv_step_dist = self._inv_step_dist
+        step_offset = self.commanded_position - start_pos * inv_step_dist
+        steps = dist * inv_step_dist
+        count = self._ffi_lib.stepcompress_push_factor(
+            self._stepqueue, steps, step_offset,
+            mcu_time * self._mcu_freq, 1. / (cruise_v * self._velocity_factor))
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self.commanded_position += count
-        return count
-    def step_factor(self, mcu_time, steps, step_offset, factor):
-        clock = mcu_time * self._mcu_freq
-        count = self._ffi_lib.stepcompress_push_factor(
-            self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
+    def step_accel(self, mcu_time, start_pos, dist, start_v, accel):
+        #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
+        inv_step_dist = self._inv_step_dist
+        mcu_freq = self._mcu_freq
+        inv_accel = 1. / accel
+        time_offset = start_v * inv_accel * mcu_freq
+        sqrt_offset = time_offset**2
+        step_offset = self.commanded_position - start_pos * inv_step_dist
+        steps = dist * inv_step_dist
+        clock = mcu_time * mcu_freq - time_offset
+        count = self._ffi_lib.stepcompress_push_sqrt(
+            self._stepqueue, steps, step_offset, clock,
+            sqrt_offset, 2. * inv_accel * self._inv_accel_factor)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self.commanded_position += count
-        return count
     def step_delta_const(self, mcu_time, dist, start_pos
                          , inv_velocity, step_dist
                          , height, closestxy_d, closest_height2, movez_r):