From 1d81bf559657422db7a7dbd7a1f7f5b2b352eb21 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 4 Apr 2017 19:54:32 -0400 Subject: [PATCH] stepcompress: Pass constant velocity and acceleration directly to C code Update the C code to take velocity and acceleration directly in step distances and clock ticks. This simplifies the mcu.py python code as it only needs to do unit and axis conversion. Signed-off-by: Kevin O'Connor --- klippy/chelper.py | 10 ++++------ klippy/mcu.py | 26 +++++++++----------------- klippy/stepcompress.c | 34 +++++++++++++++++++--------------- 3 files changed, 32 insertions(+), 38 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index b35089a1..bfc74127 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -22,12 +22,10 @@ defs_stepcompress = """ int stepcompress_push(struct stepcompress *sc, double step_clock , int32_t sdir); - int32_t stepcompress_push_factor(struct stepcompress *sc - , double steps, double step_offset - , double clock_offset, double factor); - int32_t stepcompress_push_sqrt(struct stepcompress *sc - , double steps, double step_offset - , double clock_offset, double sqrt_offset, double factor); + int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset + , double step_offset, double steps, double cruise_sv); + int32_t stepcompress_push_accel(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 , double clock_offset, double dist, double start_pos , double inv_velocity, double step_dist, double height diff --git a/klippy/mcu.py b/klippy/mcu.py index 94e94d63..9127da6c 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -29,7 +29,7 @@ class MCU_stepper: self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) self._commanded_pos = 0 self._step_dist = self._inv_step_dist = 1. - self._velocity_factor = self._inv_accel_factor = 0. + self._velocity_factor = self._accel_factor = 0. self._mcu_position_offset = 0 self._mcu_freq = self._min_stop_interval = 0. self._reset_cmd = self._get_position_cmd = None @@ -43,7 +43,7 @@ class MCU_stepper: 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 + self._accel_factor = 1. / (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( @@ -119,29 +119,21 @@ class MCU_stepper: else: self._commanded_pos -= 1 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_pos - 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)) + count = self._ffi_lib.stepcompress_push_const( + self._stepqueue, mcu_time * self._mcu_freq, step_offset, + dist * inv_step_dist, cruise_v * self._velocity_factor) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count 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_pos - 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) + count = self._ffi_lib.stepcompress_push_accel( + self._stepqueue, mcu_time * self._mcu_freq, step_offset, + dist * inv_step_dist, start_v * self._velocity_factor, + accel * self._accel_factor) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 41e7f515..6a85b3e2 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -465,11 +465,10 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) } // Schedule 'steps' number of steps with a constant time between steps -// using the formula: step_clock = clock_offset + step_num*factor +// using the formula: step_clock = clock_offset + step_num/cruise_sv int32_t -stepcompress_push_factor(struct stepcompress *sc - , double steps, double step_offset - , double clock_offset, double factor) +stepcompress_push_const(struct stepcompress *sc, double clock_offset + , double step_offset, double steps, double cruise_sv) { // Calculate number of steps to take int sdir = 1; @@ -481,8 +480,8 @@ stepcompress_push_factor(struct stepcompress *sc int count = steps + .5 - step_offset; if (count <= 0 || count > 10000000) { if (count && steps) { - errorf("push_factor invalid count %d %f %f %f %f" - , sc->oid, steps, step_offset, clock_offset, factor); + errorf("push_const invalid count %d %f %f %f %f" + , sc->oid, clock_offset, step_offset, steps, cruise_sv); return ERROR_RET; } return 0; @@ -494,6 +493,7 @@ stepcompress_push_factor(struct stepcompress *sc // Calculate each step time clock_offset += 0.5; + double factor = 1. / cruise_sv; double pos = step_offset + .5; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; while (count--) { @@ -507,11 +507,14 @@ stepcompress_push_factor(struct stepcompress *sc return res; } -// Schedule 'steps' number of steps using the formula: -// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset) +// Schedule 'steps' number of steps at constant acceleration. It uses +// the formula: +// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2) +// - start_sv/accel) int32_t -stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset - , double clock_offset, double sqrt_offset, double factor) +stepcompress_push_accel(struct stepcompress *sc, double clock_offset + , double step_offset, double steps + , double start_sv, double accel) { // Calculate number of steps to take int sdir = 1; @@ -523,9 +526,8 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset int count = steps + .5 - step_offset; if (count <= 0 || count > 10000000) { if (count && steps) { - errorf("push_sqrt invalid count %d %f %f %f %f %f" - , sc->oid, steps, step_offset, clock_offset, sqrt_offset - , factor); + errorf("push_accel invalid count %d %f %f %f %f %f" + , sc->oid, clock_offset, step_offset, steps, start_sv, accel); return ERROR_RET; } return 0; @@ -536,8 +538,10 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset int res = sdir ? count : -count; // Calculate each step time - clock_offset += 0.5; - double pos = step_offset + .5 + sqrt_offset/factor; + double inv_accel = 1. / accel; + double factor = 2. * inv_accel; + clock_offset += 0.5 - start_sv * inv_accel; + double pos = step_offset + .5 + .5 * start_sv*start_sv * inv_accel; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; while (count--) { int ret = check_expand(sc, &qn, &qend);