From 85ed5cef7fe483d921b1081b4664c35ad4e64967 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 7 Apr 2017 10:49:14 -0400 Subject: [PATCH] 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 --- klippy/chelper.py | 6 +- klippy/delta.py | 16 ++--- klippy/mcu.py | 19 +---- klippy/stepcompress.c | 159 ++++++++++++++++-------------------------- 4 files changed, 74 insertions(+), 126 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index afc5b1d9..86994830 100644 --- a/klippy/chelper.py +++ b/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 - , 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 + int32_t stepcompress_push_delta(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); diff --git a/klippy/delta.py b/klippy/delta.py index 9ae72810..5913e53a 100644 --- a/klippy/delta.py +++ b/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_time + accel_t, accel_d, cruise_up_d, cruise_v, + mcu_stepper.step_delta( + 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_time + accel_t, accel_d, -cruise_down_d, cruise_v, + mcu_stepper.step_delta( + 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) diff --git a/klippy/mcu.py b/klippy/mcu.py index 26e98d39..d28d21f1 100644 --- a/klippy/mcu.py +++ b/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 - , height_base, closestxy_d, closest_height2, movez_r): + 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( - 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( + count = self._ffi_lib.stepcompress_push_delta( 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, diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 18750e9f..4f59eaa2 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -530,85 +530,9 @@ 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( - 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) -{ - // 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_const invalid count %d %d %f %f %f %f %f %f %f %f" - , sc->oid, count, clock_offset, start_pos, steps, cruise_sv - , 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_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 (!movez_r) { - // Optmized case for common XY only moves (no Z movement) - while (count--) { - int ret = check_expand(sc, &qn, &qend); - if (ret) - return ret; - double v = safe_sqrt(closest_height2 - height*height); - double pos = start_pos + (step_dist > 0. ? -v : v); - *qn++ = clock_offset + pos * inv_cruise_sv; - height += step_dist; - } - } else if (!movexy_r) { - // Optmized case for Z only moves - double v = (step_dist > 0. ? -end_height : end_height); - while (count--) { - int ret = check_expand(sc, &qn, &qend); - if (ret) - return ret; - double pos = start_pos + movez_r*height + v; - *qn++ = clock_offset + pos * inv_cruise_sv; - height += step_dist; - } - } else { - // General case (handles XY+Z moves) - while (count--) { - int ret = check_expand(sc, &qn, &qend); - if (ret) - return ret; - double relheight = movexy_r*height - movez_r*closestxy_sd; - double v = safe_sqrt(closest_height2 - relheight*relheight); - double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); - *qn++ = clock_offset + pos * inv_cruise_sv; - height += step_dist; - } - } - sc->queue_next = qn; - return res; -} - -// Schedule 'count' number of steps using delta kinematic acceleration -int32_t -stepcompress_push_delta_accel( +stepcompress_push_delta( 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) @@ -625,11 +549,9 @@ stepcompress_push_delta_accel( 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); + errorf("push_delta 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; @@ -640,22 +562,65 @@ stepcompress_push_delta_accel( int res = step_dist > 0. ? count : -count; // Calculate each step time - double inv_accel = 1. / 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; + clock_offset += 0.5; + start_pos += movexy_r*closestxy_sd; 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) - return ret; - double relheight = movexy_r*height - movez_r*closestxy_sd; - double v = safe_sqrt(closest_height2 - relheight*relheight); - double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); - v = safe_sqrt(pos * accel_multiplier); - *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v); - height += step_dist; + 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--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + double v = safe_sqrt(closest_height2 - height*height); + double pos = start_pos + (step_dist > 0. ? -v : v); + *qn++ = clock_offset + pos * inv_cruise_sv; + height += step_dist; + } + } else if (!movexy_r) { + // Optmized case for Z only moves + double v = (step_dist > 0. ? -end_height : end_height); + while (count--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + double pos = start_pos + movez_r*height + v; + *qn++ = clock_offset + pos * inv_cruise_sv; + height += step_dist; + } + } else { + // General case (handles XY+Z moves) + while (count--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + double relheight = movexy_r*height - movez_r*closestxy_sd; + double v = safe_sqrt(closest_height2 - relheight*relheight); + double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); + *qn++ = clock_offset + pos * inv_cruise_sv; + height += step_dist; + } + } + } else { + // Move with constant acceleration + 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; + while (count--) { + int ret = check_expand(sc, &qn, &qend); + if (ret) + return ret; + double relheight = movexy_r*height - movez_r*closestxy_sd; + double v = safe_sqrt(closest_height2 - relheight*relheight); + double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); + v = safe_sqrt(pos * accel_multiplier); + *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v); + height += step_dist; + } } sc->queue_next = qn; return res;