diff options
Diffstat (limited to 'klippy')
| -rw-r--r-- | klippy/chelper.py | 12 | ||||
| -rw-r--r-- | klippy/mcu.py | 35 | ||||
| -rw-r--r-- | 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); |