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

stepcompress: Merge stepcompress_delta_const and stepcompress_delta_accel 

It's not necessary to have separate C delta kinematic functions for
constant acceleration and constant velocity as constant velocity can
be obtained by using a constant acceleration of zero.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-07 10:49:14 -04:00
parent df42b0d1ac
commit 85ed5cef7f
4 changed files with 74 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
klippy/chelper.py
View File

@ -24,11 +24,7 @@ defs_stepcompress = """
, int32_t sdir);
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 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
int32_t stepcompress_push_delta(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);

16
klippy/delta.py
View File

@ -228,28 +228,28 @@ class DeltaKinematics:
mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
if accel_up_d > 0.:
mcu_stepper.step_delta_accel(
mcu_stepper.step_delta(
mcu_time, 0., accel_up_d, move.start_v, accel,
origz, closestxy_d, closest_height2, movez_r)
if cruise_up_d > 0.:
mcu_stepper.step_delta_const(
mcu_time + accel_t, accel_d, cruise_up_d, cruise_v,
mcu_stepper.step_delta(
mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, 0.,
origz, closestxy_d, closest_height2, movez_r)
if decel_up_d > 0.:
mcu_stepper.step_delta_accel(
mcu_stepper.step_delta(
mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
cruise_v, -accel,
origz, closestxy_d, closest_height2, movez_r)
if accel_down_d > 0.:
mcu_stepper.step_delta_accel(
mcu_stepper.step_delta(
mcu_time, 0., -accel_down_d, move.start_v, accel,
origz, closestxy_d, closest_height2, movez_r)
if cruise_down_d > 0.:
mcu_stepper.step_delta_const(
mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v,
mcu_stepper.step_delta(
mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, 0.,
origz, closestxy_d, closest_height2, movez_r)
if decel_down_d > 0.:
mcu_stepper.step_delta_accel(
mcu_stepper.step_delta(
mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
cruise_v, -accel,
origz, closestxy_d, closest_height2, movez_r)

19
klippy/mcu.py
View File

@ -128,24 +128,11 @@ class MCU_stepper:
if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress")
self._commanded_pos += count
def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
, height_base, closestxy_d, closest_height2, movez_r):
def step_delta(self, mcu_time, start_pos, dist, start_v, accel
, height_base, closestxy_d, closest_height2, movez_r):
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 * 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
height = self._commanded_pos - height_base * inv_step_dist
count = self._ffi_lib.stepcompress_push_delta_accel(
count = self._ffi_lib.stepcompress_push_delta(
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,

159
klippy/stepcompress.c
View File

@ -530,85 +530,9 @@ stepcompress_push_const(
return res;
}
// Schedule 'count' number of steps using the delta kinematic const speed
// Schedule steps using delta kinematics
int32_t
stepcompress_push_delta_const(
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_sd - movexy_r*steps;
double end_height = safe_sqrt(closest_height2 - reldist*reldist);
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, start_pos, steps, cruise_sv
, height, closestxy_sd, closest_height2, movez_r);
return ERROR_RET;
}
return 0;
}
int ret = set_next_step_dir(sc, step_dist > 0.);
if (ret)
return ret;
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_sd;
height += .5 * step_dist;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
if (!movez_r) {
// Optmized case for common XY only moves (no Z movement)
while (count--) {
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double v = safe_sqrt(closest_height2 - height*height);
double pos = start_pos + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_cruise_sv;
height += step_dist;
}
} else if (!movexy_r) {
// Optmized case for Z only moves
double v = (step_dist > 0. ? -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;
}
} else {
// General case (handles XY+Z moves)
while (count--) {
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
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_cruise_sv;
height += step_dist;
}
}
sc->queue_next = qn;
return res;
}
// Schedule 'count' number of steps using delta kinematic acceleration
int32_t
stepcompress_push_delta_accel(
stepcompress_push_delta(
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)
@ -625,11 +549,9 @@ stepcompress_push_delta_accel(
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 %f"
, sc->oid, count, clock_offset, start_pos
, steps, start_sv, accel
, height, closestxy_sd, closest_height2, movez_r);
errorf("push_delta 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;
@ -640,22 +562,65 @@ stepcompress_push_delta_accel(
int res = step_dist > 0. ? count : -count;
// Calculate each step time
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;
clock_offset += 0.5;
start_pos += movexy_r*closestxy_sd;
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_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);
*qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
height += step_dist;
if (!accel) {
// Move at constant velocity (zero acceleration)
double inv_cruise_sv = 1. / start_sv;
if (!movez_r) {
// Optmized case for common XY only moves (no Z movement)
while (count--) {
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
double v = safe_sqrt(closest_height2 - height*height);
double pos = start_pos + (step_dist > 0. ? -v : v);
*qn++ = clock_offset + pos * inv_cruise_sv;
height += step_dist;
}
} else if (!movexy_r) {
// Optmized case for Z only moves
double v = (step_dist > 0. ? -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;
}
} else {
// General case (handles XY+Z moves)
while (count--) {
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
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_cruise_sv;
height += step_dist;
}
}
} else {
// Move with constant acceleration
double inv_accel = 1. / accel;
clock_offset -= start_sv * inv_accel;
start_pos += 0.5 * start_sv*start_sv * inv_accel;
double accel_multiplier = 2. * inv_accel;
while (count--) {
int ret = check_expand(sc, &qn, &qend);
if (ret)
return ret;
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);
*qn++ = clock_offset + (accel_multiplier >= 0. ? v : -v);
height += step_dist;
}
}
sc->queue_next = qn;
return res;