diff options
| -rw-r--r-- | klippy/stepcompress.c | 41 |
1 files changed, 18 insertions, 23 deletions
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 761c16c2..87f5a45f 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -533,21 +533,16 @@ stepcompress_push_const( // Schedule steps using delta kinematics static int32_t _stepcompress_push_delta( - struct stepcompress *sc, double clock_offset, double move_sd - , double start_sv, double accel + struct stepcompress *sc, int sdir + , double clock_offset, double move_sd, double start_sv, double accel , double height, double startxy_sd, double arm_sd, double movez_r) { // Calculate number of steps to take - double step_dist = 1.; - if (move_sd < 0) { - step_dist = -1.; - move_sd = -move_sd; - } double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; double arm_sd2 = arm_sd * arm_sd; double endxy_sd = startxy_sd - movexy_r*move_sd; double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd); - int count = (end_height + movez_r*move_sd - height) * step_dist + .5; + int count = (end_height + movez_r*move_sd - height) * (sdir ? 1. : -1.) + .5; if (count <= 0 || count > 10000000) { if (count) { errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f" @@ -557,15 +552,15 @@ _stepcompress_push_delta( } return 0; } - int ret = set_next_step_dir(sc, step_dist > 0.); + int ret = set_next_step_dir(sc, sdir); if (ret) return ret; - int res = step_dist > 0. ? count : -count; + int res = sdir ? count : -count; // Calculate each step time clock_offset += 0.5; + height += (sdir ? .5 : -.5); double start_pos = movexy_r*startxy_sd; - height += .5 * step_dist; uint64_t *qn = sc->queue_next, *qend = sc->queue_end; if (!accel) { // Move at constant velocity (zero acceleration) @@ -577,20 +572,20 @@ _stepcompress_push_delta( if (ret) return ret; double v = safe_sqrt(arm_sd2 - height*height); - double pos = start_pos + (step_dist > 0. ? -v : v); + double pos = start_pos + (sdir ? -v : v); *qn++ = clock_offset + pos * inv_cruise_sv; - height += step_dist; + height += (sdir ? 1. : -1.); } } else if (!movexy_r) { // Optmized case for Z only moves - double v = (step_dist > 0. ? -end_height : end_height); + double v = (sdir ? -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; + height += (sdir ? 1. : -1.); } } else { // General case (handles XY+Z moves) @@ -600,9 +595,9 @@ _stepcompress_push_delta( return ret; double relheight = movexy_r*height - movez_r*startxy_sd; double v = safe_sqrt(arm_sd2 - relheight*relheight); - double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); + double pos = start_pos + movez_r*height + (sdir ? -v : v); *qn++ = clock_offset + pos * inv_cruise_sv; - height += step_dist; + height += (sdir ? 1. : -1.); } } } else { @@ -617,10 +612,10 @@ _stepcompress_push_delta( return ret; double relheight = movexy_r*height - movez_r*startxy_sd; double v = safe_sqrt(arm_sd2 - relheight*relheight); - double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); + double pos = start_pos + movez_r*height + (sdir ? -v : v); v = safe_sqrt(pos * accel_multiplier); *qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v); - height += step_dist; + height += (sdir ? 1. : -1.); } } sc->queue_next = qn; @@ -637,22 +632,22 @@ stepcompress_push_delta( if (reversexy_sd <= 0.) // All steps are in down direction return _stepcompress_push_delta( - sc, clock_offset, -move_sd, start_sv, accel + sc, 0, clock_offset, move_sd, start_sv, accel , height, startxy_sd, arm_sd, movez_r); double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; if (reversexy_sd >= move_sd * movexy_r) // All steps are in up direction return _stepcompress_push_delta( - sc, clock_offset, move_sd, start_sv, accel + sc, 1, clock_offset, move_sd, start_sv, accel , height, startxy_sd, arm_sd, movez_r); // Steps in both up and down direction int res1 = _stepcompress_push_delta( - sc, clock_offset, reversexy_sd / movexy_r, start_sv, accel + sc, 1, clock_offset, reversexy_sd / movexy_r, start_sv, accel , height, startxy_sd, arm_sd, movez_r); if (res1 == ERROR_RET) return res1; int res2 = _stepcompress_push_delta( - sc, clock_offset, -move_sd, start_sv, accel + sc, 0, clock_offset, move_sd, start_sv, accel , height + res1, startxy_sd, arm_sd, movez_r); if (res2 == ERROR_RET) return res2; |