diff options
Diffstat (limited to 'klippy/stepcompress.c')
| -rw-r--r-- | klippy/stepcompress.c | 61 |
1 files changed, 38 insertions, 23 deletions
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); |