From df42b0d1ac0b303025c8762fa727d79bc01a28a3 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 4 Apr 2017 21:59:28 -0400 Subject: [PATCH] stepcompress: Pass delta velocity and acceleration directly to C code Update the C delta kinematic 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 | 12 ++++----- klippy/mcu.py | 35 +++++++++---------------- klippy/stepcompress.c | 61 +++++++++++++++++++++++++++---------------- 3 files changed, 57 insertions(+), 51 deletions(-) diff --git a/klippy/chelper.py b/klippy/chelper.py index 910fa5e6..afc5b1d9 100644 --- a/klippy/chelper.py +++ b/klippy/chelper.py @@ -25,13 +25,13 @@ defs_stepcompress = """ 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 dist, double start_pos - , double inv_velocity, double step_dist, double height - , double closestxy_d, double closest_height2, double movez_r); + , 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 dist, double start_pos - , double accel_multiplier, double step_dist, double height - , double closestxy_d, double closest_height2, double movez_r); + , double clock_offset, double start_pos, double steps, double start_sv + , double accel, double height, double closestxy_sd + , double closest_height2, double movez_r); struct steppersync *steppersync_alloc(struct serialqueue *sq , struct stepcompress **sc_list, int sc_num, int move_num); diff --git a/klippy/mcu.py b/klippy/mcu.py index 741ac0aa..26e98d39 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -130,36 +130,27 @@ class MCU_stepper: self._commanded_pos += count def step_delta_const(self, mcu_time, start_pos, dist, cruise_v , height_base, closestxy_d, closest_height2, movez_r): - mcu_freq = self._mcu_freq - step_dist = self._step_dist - height = self._commanded_pos*step_dist - height_base - if dist < 0: - dist = -dist - step_dist = -step_dist + 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 * mcu_freq, dist, -start_pos - , mcu_freq / cruise_v, step_dist - , height, closestxy_d, closest_height2, movez_r) + 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 - mcu_freq = self._mcu_freq - inv_accel = 1. / accel - time_offset = start_v * inv_accel * mcu_freq - accel_offset = start_v**2 * 0.5 * inv_accel - step_dist = self._step_dist - height = self._commanded_pos*step_dist - height_base - if dist < 0: - dist = -dist - step_dist = -step_dist - clock = mcu_time * mcu_freq - time_offset + height = self._commanded_pos - height_base * inv_step_dist count = self._ffi_lib.stepcompress_push_delta_accel( - self._stepqueue, clock, dist, accel_offset - start_pos - , 2. * inv_accel * mcu_freq**2, step_dist - , height, closestxy_d, closest_height2, movez_r) + 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, + 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 diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 8c847cb3..18750e9f 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -533,20 +533,25 @@ stepcompress_push_const( // Schedule 'count' number of steps using the delta kinematic const speed 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, double closestxy_d, double closest_height2, double movez_r) + 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_d - movexy_r*dist; + double reldist = closestxy_sd - movexy_r*steps; double end_height = safe_sqrt(closest_height2 - reldist*reldist); - int count = (end_height - height + movez_r*dist) / step_dist + .5; + 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, dist, step_dist, start_pos - , closest_height2, height, movez_r, inv_velocity); + , sc->oid, count, clock_offset, start_pos, steps, cruise_sv + , height, closestxy_sd, closest_height2, movez_r); return ERROR_RET; } return 0; @@ -557,8 +562,9 @@ 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_d; + start_pos += movexy_r*closestxy_sd; height += .5 * step_dist; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; if (!movez_r) { @@ -569,7 +575,7 @@ stepcompress_push_delta_const( return ret; double v = safe_sqrt(closest_height2 - height*height); double pos = start_pos + (step_dist > 0. ? -v : v); - *qn++ = clock_offset + pos * inv_velocity; + *qn++ = clock_offset + pos * inv_cruise_sv; height += step_dist; } } else if (!movexy_r) { @@ -580,7 +586,7 @@ stepcompress_push_delta_const( if (ret) return ret; double pos = start_pos + movez_r*height + v; - *qn++ = clock_offset + pos * inv_velocity; + *qn++ = clock_offset + pos * inv_cruise_sv; height += step_dist; } } else { @@ -589,10 +595,10 @@ stepcompress_push_delta_const( int ret = check_expand(sc, &qn, &qend); if (ret) return ret; - double relheight = movexy_r*height - movez_r*closestxy_d; + 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_velocity; + *qn++ = clock_offset + pos * inv_cruise_sv; height += step_dist; } } @@ -603,20 +609,27 @@ stepcompress_push_delta_const( // Schedule 'count' number of steps using delta kinematic acceleration int32_t stepcompress_push_delta_accel( - struct stepcompress *sc, double clock_offset, double dist, double start_pos - , double accel_multiplier, double step_dist - , double height, double closestxy_d, double closest_height2, double movez_r) + 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_d - movexy_r*dist; + double reldist = closestxy_sd - movexy_r*steps; double end_height = safe_sqrt(closest_height2 - reldist*reldist); - int count = (end_height - height + movez_r*dist) / step_dist + .5; + 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" - , sc->oid, count, clock_offset, dist, step_dist, start_pos - , closest_height2, height, movez_r, accel_multiplier); + 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; @@ -627,15 +640,17 @@ stepcompress_push_delta_accel( int res = step_dist > 0. ? count : -count; // Calculate each step time - clock_offset += 0.5; - start_pos += movexy_r*closestxy_d; + 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; 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_d; + 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);