From 98add22891d66d0a9ce21d35b750d20d3671382e Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 7 Apr 2017 12:51:52 -0400 Subject: [PATCH] stepcompress: Merge stepcompress_push_accel() and stepcompress_push_const() It's not necessary to have separate C 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 --- klippy/cartesian.py | 7 ++-- klippy/chelper.py | 2 - klippy/corexy.py | 7 ++-- klippy/extruder.py | 8 ++-- klippy/mcu.py | 11 +---- klippy/stepcompress.c | 95 ++++++++++++++++--------------------------- 6 files changed, 48 insertions(+), 82 deletions(-) diff --git a/klippy/cartesian.py b/klippy/cartesian.py index c1e07a84..ab95df5d 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -112,18 +112,19 @@ class CartKinematics: # Acceleration steps if move.accel_r: accel_d = move.accel_r * axis_d - mcu_stepper.step_accel( + mcu_stepper.step_const( mcu_time, start_pos, accel_d, move.start_v * axis_r, accel) start_pos += accel_d mcu_time += move.accel_t # Cruising steps if move.cruise_r: cruise_d = move.cruise_r * axis_d - mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) + mcu_stepper.step_const( + mcu_time, start_pos, cruise_d, cruise_v, 0.) start_pos += cruise_d mcu_time += move.cruise_t # Deceleration steps if move.decel_r: decel_d = move.decel_r * axis_d - mcu_stepper.step_accel( + mcu_stepper.step_const( mcu_time, start_pos, decel_d, cruise_v, -accel) diff --git a/klippy/chelper.py b/klippy/chelper.py index bfc74127..910fa5e6 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -23,8 +23,6 @@ defs_stepcompress = """ int stepcompress_push(struct stepcompress *sc, double step_clock , int32_t sdir); 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 diff --git a/klippy/corexy.py b/klippy/corexy.py index b724a4e1..dfee7bb3 100644 --- a/klippy/corexy.py +++ b/klippy/corexy.py @@ -126,18 +126,19 @@ class CoreXYKinematics: # Acceleration steps if move.accel_r: accel_d = move.accel_r * axis_d - mcu_stepper.step_accel( + mcu_stepper.step_const( mcu_time, start_pos, accel_d, move.start_v * axis_r, accel) start_pos += accel_d mcu_time += move.accel_t # Cruising steps if move.cruise_r: cruise_d = move.cruise_r * axis_d - mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) + mcu_stepper.step_const( + mcu_time, start_pos, cruise_d, cruise_v, 0.) start_pos += cruise_d mcu_time += move.cruise_t # Deceleration steps if move.decel_r: decel_d = move.decel_r * axis_d - mcu_stepper.step_accel( + mcu_stepper.step_const( mcu_time, start_pos, decel_d, cruise_v, -accel) diff --git a/klippy/extruder.py b/klippy/extruder.py index 4321f06b..24153e0f 100644 --- a/klippy/extruder.py +++ b/klippy/extruder.py @@ -169,22 +169,22 @@ class PrinterExtruder: # Acceleration steps if accel_d: - mcu_stepper.step_accel(mcu_time, start_pos, accel_d, start_v, accel) + mcu_stepper.step_const(mcu_time, start_pos, accel_d, start_v, accel) start_pos += accel_d mcu_time += accel_t # Cruising steps if cruise_d: - mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v) + mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v, 0.) start_pos += cruise_d mcu_time += cruise_t # Deceleration steps if decel_d: - mcu_stepper.step_accel(mcu_time, start_pos, decel_d, decel_v, -accel) + mcu_stepper.step_const(mcu_time, start_pos, decel_d, decel_v, -accel) start_pos += decel_d mcu_time += decel_t # Retraction steps if retract_d: - mcu_stepper.step_accel( + mcu_stepper.step_const( mcu_time, start_pos, -retract_d, retract_v, accel) start_pos -= retract_d self.extrude_pos = start_pos diff --git a/klippy/mcu.py b/klippy/mcu.py index 9127da6c..741ac0aa 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -118,19 +118,10 @@ class MCU_stepper: self._commanded_pos += 1 else: self._commanded_pos -= 1 - def step_const(self, mcu_time, start_pos, dist, cruise_v): + def step_const(self, mcu_time, start_pos, dist, start_v, accel): inv_step_dist = self._inv_step_dist step_offset = self._commanded_pos - start_pos * inv_step_dist 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): - inv_step_dist = self._inv_step_dist - step_offset = self._commanded_pos - start_pos * inv_step_dist - 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) diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 6a85b3e2..8c847cb3 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -464,11 +464,16 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir) return 0; } -// Schedule 'steps' number of steps with a constant time between steps -// using the formula: step_clock = clock_offset + step_num/cruise_sv +// Schedule 'steps' number of steps at constant acceleration. If +// acceleration is zero (ie, constant velocity) it uses the formula: +// step_clock = clock_offset + step_num/start_sv +// Otherwise it uses the formula: +// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2) +// - start_sv/accel) int32_t -stepcompress_push_const(struct stepcompress *sc, double clock_offset - , double step_offset, double steps, double cruise_sv) +stepcompress_push_const( + 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; @@ -480,8 +485,9 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset int count = steps + .5 - step_offset; if (count <= 0 || count > 10000000) { if (count && steps) { - errorf("push_const invalid count %d %f %f %f %f" - , sc->oid, clock_offset, step_offset, steps, cruise_sv); + errorf("push_const invalid count %d %f %f %f %f %f" + , sc->oid, clock_offset, step_offset, steps + , start_sv, accel); return ERROR_RET; } return 0; @@ -493,63 +499,32 @@ stepcompress_push_const(struct stepcompress *sc, double clock_offset // 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--) { - int ret = check_expand(sc, &qn, &qend); - if (ret) - return ret; - *qn++ = clock_offset + pos*factor; - pos += 1.0; - } - sc->queue_next = qn; - return res; -} - -// 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_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; - if (steps < 0) { - sdir = 0; - steps = -steps; - step_offset = -step_offset; - } - int count = steps + .5 - step_offset; - if (count <= 0 || count > 10000000) { - if (count && steps) { - errorf("push_accel invalid count %d %f %f %f %f %f" - , sc->oid, clock_offset, step_offset, steps, start_sv, accel); - return ERROR_RET; + if (!accel) { + // Move at constant velocity (zero acceleration) + double inv_cruise_sv = 1. / start_sv; + while (count--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + *qn++ = clock_offset + pos*inv_cruise_sv; + pos += 1.0; + } + } else { + // Move with constant acceleration + double inv_accel = 1. / accel; + clock_offset -= start_sv * inv_accel; + pos += .5 * start_sv*start_sv * inv_accel; + double accel_multiplier = 2. * inv_accel; + while (count--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + double v = safe_sqrt(pos * accel_multiplier); + *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v); + pos += 1.0; } - return 0; - } - int ret = set_next_step_dir(sc, sdir); - if (ret) - return ret; - int res = sdir ? count : -count; - - // Calculate each step time - 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); - if (ret) - return ret; - double v = safe_sqrt(pos*factor); - *qn++ = clock_offset + (factor >= 0. ? v : -v); - pos += 1.0; } sc->queue_next = qn; return res;