3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stepcompress: Remove step_dist from stepcompress_push_delta() 

Pass the step direction explicitly to the low-level delta kinematic C
code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-07 11:55:26 -04:00
parent 33b809714f
commit d7a0e22d59
1 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
klippy/stepcompress.c
View File

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