stepcompress: Merge stepcompress_delta_const and stepcompress_delta_accel

It's not necessary to have separate C delta kinematic functions for
constant acceleration and constant velocity as constant velocity can
be obtained by using a constant acceleration of zero.

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

klippy/chelper.py

@@ -24,11 +24,7 @@ defs_stepcompress = """
         , int32_t sdir);
     int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
         , double step_offset, double steps, double start_sv, double accel);
-    int32_t stepcompress_push_delta_const(struct stepcompress *sc
+    int32_t stepcompress_push_delta(struct stepcompress *sc
-        , double clock_offset, double start_pos, double steps, double cruise_sv
-        , double height, double closestxy_sd
-        , double closest_height2, double movez_r);
-    int32_t stepcompress_push_delta_accel(struct stepcompress *sc
         , double clock_offset, double start_pos, double steps, double start_sv
         , double accel, double height, double closestxy_sd
         , double closest_height2, double movez_r);

klippy/delta.py

@@ -228,28 +228,28 @@ class DeltaKinematics:
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             if accel_up_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time, 0., accel_up_d, move.start_v, accel,
                     origz, closestxy_d, closest_height2, movez_r)
             if cruise_up_d > 0.:
-                mcu_stepper.step_delta_const(
+                mcu_stepper.step_delta(
-                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v,
+                    mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0.,
                     origz, closestxy_d, closest_height2, movez_r)
             if decel_up_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     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.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time, 0., -accel_down_d, move.start_v, accel,
                     origz, closestxy_d, closest_height2, movez_r)
             if cruise_down_d > 0.:
-                mcu_stepper.step_delta_const(
+                mcu_stepper.step_delta(
-                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v,
+                    mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, 0.,
                     origz, closestxy_d, closest_height2, movez_r)
             if decel_down_d > 0.:
-                mcu_stepper.step_delta_accel(
+                mcu_stepper.step_delta(
                     mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
                     cruise_v, -accel,
                     origz, closestxy_d, closest_height2, movez_r)

klippy/mcu.py

@@ -128,24 +128,11 @@ class MCU_stepper:
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
-    def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
+    def step_delta(self, mcu_time, start_pos, dist, start_v, accel
                    , height_base, closestxy_d, closest_height2, movez_r):
         inv_step_dist = self._inv_step_dist
         height = self._commanded_pos - height_base * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_delta_const(
+        count = self._ffi_lib.stepcompress_push_delta(
-            self._stepqueue, mcu_time * self._mcu_freq,
-            -start_pos * inv_step_dist, dist * inv_step_dist,
-            cruise_v * self._velocity_factor,
-            height, closestxy_d * inv_step_dist,
-            closest_height2 * inv_step_dist**2, movez_r)
-        if count == STEPCOMPRESS_ERROR_RET:
-            raise error("Internal error in stepcompress")
-        self._commanded_pos += count
-    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
-        height = self._commanded_pos - height_base * inv_step_dist
-        count = self._ffi_lib.stepcompress_push_delta_accel(
             self._stepqueue, mcu_time * self._mcu_freq,
             -start_pos * inv_step_dist, dist * inv_step_dist,
             start_v * self._velocity_factor, accel * self._accel_factor,

klippy/stepcompress.c

@@ -530,11 +530,11 @@ stepcompress_push_const(
     return res;
 }
 
-// Schedule 'count' number of steps using the delta kinematic const speed
+// Schedule steps using delta kinematics
 int32_t
-stepcompress_push_delta_const(
+stepcompress_push_delta(
     struct stepcompress *sc, double clock_offset, double start_pos
-    , double steps, double cruise_sv
+    , double steps, double start_sv, double accel
     , double height, double closestxy_sd, double closest_height2, double movez_r)
 {
     // Calculate number of steps to take
@@ -549,9 +549,9 @@ stepcompress_push_delta_const(
     int count = (end_height - height + movez_r*steps) * step_dist + .5;
     if (count <= 0 || count > 10000000) {
         if (count) {
-            errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
+            errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f %f"
-                   , sc->oid, count, clock_offset, start_pos, steps, cruise_sv
+                   , sc->oid, count, clock_offset, start_pos, steps, start_sv
-                   , height, closestxy_sd, closest_height2, movez_r);
+                   , accel, height, closestxy_sd, closest_height2, movez_r);
             return ERROR_RET;
         }
         return 0;
@@ -562,11 +562,13 @@ stepcompress_push_delta_const(
     int res = step_dist > 0. ? count : -count;
 
     // Calculate each step time
-    double inv_cruise_sv = 1. / cruise_sv;
     clock_offset += 0.5;
     start_pos += movexy_r*closestxy_sd;
     height += .5 * step_dist;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
+    if (!accel) {
+        // Move at constant velocity (zero acceleration)
+        double inv_cruise_sv = 1. / start_sv;
         if (!movez_r) {
             // Optmized case for common XY only moves (no Z movement)
             while (count--) {
@@ -602,50 +604,12 @@ stepcompress_push_delta_const(
                 height += step_dist;
             }
         }
-    sc->queue_next = qn;
+    } else {
-    return res;
+        // Move with constant acceleration
-}
-
-// Schedule 'count' number of steps using delta kinematic acceleration
-int32_t
-stepcompress_push_delta_accel(
-    struct stepcompress *sc, double clock_offset, double start_pos
-    , double steps, double start_sv, double accel
-    , double height, double closestxy_sd, double closest_height2, double movez_r)
-{
-    // Calculate number of steps to take
-    double step_dist = 1.;
-    if (steps < 0) {
-        step_dist = -1.;
-        steps = -steps;
-    }
-    double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
-    double reldist = closestxy_sd - movexy_r*steps;
-    double end_height = safe_sqrt(closest_height2 - reldist*reldist);
-    int count = (end_height - height + movez_r*steps) * step_dist + .5;
-    if (count <= 0 || count > 10000000) {
-        if (count) {
-            errorf("push_delta_accel invalid count %d %d %f %f"
-                   " %f %f %f %f %f %f %f"
-                   , sc->oid, count, clock_offset, start_pos
-                   , steps, start_sv, accel
-                   , height, closestxy_sd, closest_height2, movez_r);
-            return ERROR_RET;
-        }
-        return 0;
-    }
-    int ret = set_next_step_dir(sc, step_dist > 0.);
-    if (ret)
-        return ret;
-    int res = step_dist > 0. ? count : -count;
-
-    // Calculate each step time
         double inv_accel = 1. / accel;
+        clock_offset -= start_sv * inv_accel;
+        start_pos += 0.5 * start_sv*start_sv * inv_accel;
         double accel_multiplier = 2. * inv_accel;
-    clock_offset += 0.5 - start_sv * inv_accel;
-    start_pos += movexy_r*closestxy_sd + 0.5 * start_sv*start_sv * inv_accel;
-    height += .5 * step_dist;
-    uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
         while (count--) {
             int ret = check_expand(sc, &qn, &qend);
             if (ret)
@@ -657,6 +621,7 @@ stepcompress_push_delta_accel(
             *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
             height += step_dist;
         }
+    }
     sc->queue_next = qn;
     return res;
 }