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

stepcompress: Pass delta velocity and acceleration directly to C code 

Update the C delta kinematic code to take velocity and acceleration
directly in step distances and clock ticks.  This simplifies the
mcu.py python code as it only needs to do unit and axis conversion.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-04 21:59:28 -04:00
parent 98add22891
commit df42b0d1ac
3 changed files with 57 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
klippy/chelper.py
View File

@ -25,13 +25,13 @@ defs_stepcompress = """
int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
, double step_offset, double steps, double start_sv, double accel); , double step_offset, double steps, double start_sv, double accel);
int32_t stepcompress_push_delta_const(struct stepcompress *sc int32_t stepcompress_push_delta_const(struct stepcompress *sc
, double clock_offset, double dist, double start_pos , double clock_offset, double start_pos, double steps, double cruise_sv
, double inv_velocity, double step_dist, double height , double height, double closestxy_sd
, double closestxy_d, double closest_height2, double movez_r); , double closest_height2, double movez_r);
int32_t stepcompress_push_delta_accel(struct stepcompress *sc int32_t stepcompress_push_delta_accel(struct stepcompress *sc
, double clock_offset, double dist, double start_pos , double clock_offset, double start_pos, double steps, double start_sv
, double accel_multiplier, double step_dist, double height , double accel, double height, double closestxy_sd
, double closestxy_d, double closest_height2, double movez_r); , double closest_height2, double movez_r);
struct steppersync *steppersync_alloc(struct serialqueue *sq struct steppersync *steppersync_alloc(struct serialqueue *sq
, struct stepcompress **sc_list, int sc_num, int move_num); , struct stepcompress **sc_list, int sc_num, int move_num);

35
klippy/mcu.py
View File

@ -130,36 +130,27 @@ class MCU_stepper:
self._commanded_pos += count self._commanded_pos += count
def step_delta_const(self, mcu_time, start_pos, dist, cruise_v def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
, height_base, closestxy_d, closest_height2, movez_r): , height_base, closestxy_d, closest_height2, movez_r):
mcu_freq = self._mcu_freq inv_step_dist = self._inv_step_dist
step_dist = self._step_dist height = self._commanded_pos - height_base * inv_step_dist
height = self._commanded_pos*step_dist - height_base
if dist < 0:
dist = -dist
step_dist = -step_dist
count = self._ffi_lib.stepcompress_push_delta_const( count = self._ffi_lib.stepcompress_push_delta_const(
self._stepqueue, mcu_time * mcu_freq, dist, -start_pos self._stepqueue, mcu_time * self._mcu_freq,
, mcu_freq / cruise_v, step_dist -start_pos * inv_step_dist, dist * inv_step_dist,
, height, closestxy_d, closest_height2, movez_r) cruise_v * self._velocity_factor,
height, closestxy_d * inv_step_dist,
closest_height2 * inv_step_dist**2, movez_r)
if count == STEPCOMPRESS_ERROR_RET: if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
self._commanded_pos += count self._commanded_pos += count
def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel
, height_base, closestxy_d, closest_height2, movez_r): , height_base, closestxy_d, closest_height2, movez_r):
inv_step_dist = self._inv_step_dist inv_step_dist = self._inv_step_dist
mcu_freq = self._mcu_freq height = self._commanded_pos - height_base * inv_step_dist
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
count = self._ffi_lib.stepcompress_push_delta_accel( count = self._ffi_lib.stepcompress_push_delta_accel(
self._stepqueue, clock, dist, accel_offset - start_pos self._stepqueue, mcu_time * self._mcu_freq,
, 2. * inv_accel * mcu_freq**2, step_dist -start_pos * inv_step_dist, dist * inv_step_dist,
, height, closestxy_d, closest_height2, movez_r) 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: if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
self._commanded_pos += count self._commanded_pos += count

61
klippy/stepcompress.c
View File

@ -533,20 +533,25 @@ stepcompress_push_const(
// Schedule 'count' number of steps using the delta kinematic const speed // Schedule 'count' number of steps using the delta kinematic const speed
int32_t int32_t
stepcompress_push_delta_const( stepcompress_push_delta_const(
struct stepcompress *sc, double clock_offset, double dist, double start_pos struct stepcompress *sc, double clock_offset, double start_pos
, double inv_velocity, double step_dist , double steps, double cruise_sv
, double height, double closestxy_d, double closest_height2, double movez_r) , double height, double closestxy_sd, double closest_height2, double movez_r)
{ {
// Calculate number of steps to take // 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 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); 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 <= 0 || count > 10000000) {
if (count) { if (count) {
errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f" 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 , sc->oid, count, clock_offset, start_pos, steps, cruise_sv
, closest_height2, height, movez_r, inv_velocity); , height, closestxy_sd, closest_height2, movez_r);
return ERROR_RET; return ERROR_RET;
} }
return 0; return 0;
@ -557,8 +562,9 @@ stepcompress_push_delta_const(
int res = step_dist > 0. ? count : -count; int res = step_dist > 0. ? count : -count;
// Calculate each step time // Calculate each step time
double inv_cruise_sv = 1. / cruise_sv;
clock_offset += 0.5; clock_offset += 0.5;
start_pos += movexy_r*closestxy_d; start_pos += movexy_r*closestxy_sd;
height += .5 * step_dist; 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 (!movez_r) { if (!movez_r) {
@ -569,7 +575,7 @@ stepcompress_push_delta_const(
return ret; return ret;
double v = safe_sqrt(closest_height2 - height*height); double v = safe_sqrt(closest_height2 - height*height);
double pos = start_pos + (step_dist > 0. ? -v : v); 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; height += step_dist;
} }
} else if (!movexy_r) { } else if (!movexy_r) {
@ -580,7 +586,7 @@ stepcompress_push_delta_const(
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_velocity; *qn++ = clock_offset + pos * inv_cruise_sv;
height += step_dist; height += step_dist;
} }
} else { } else {
@ -589,10 +595,10 @@ stepcompress_push_delta_const(
int ret = check_expand(sc, &qn, &qend); int ret = check_expand(sc, &qn, &qend);
if (ret) if (ret)
return 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 v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); 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; height += step_dist;
} }
} }
@ -603,20 +609,27 @@ stepcompress_push_delta_const(
// Schedule 'count' number of steps using delta kinematic acceleration // Schedule 'count' number of steps using delta kinematic acceleration
int32_t int32_t
stepcompress_push_delta_accel( stepcompress_push_delta_accel(
struct stepcompress *sc, double clock_offset, double dist, double start_pos struct stepcompress *sc, double clock_offset, double start_pos
, double accel_multiplier, double step_dist , double steps, double start_sv, double accel
, double height, double closestxy_d, double closest_height2, double movez_r) , double height, double closestxy_sd, double closest_height2, double movez_r)
{ {
// Calculate number of steps to take // 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 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); 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 <= 0 || count > 10000000) {
if (count) { if (count) {
errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f" errorf("push_delta_accel invalid count %d %d %f %f"
, sc->oid, count, clock_offset, dist, step_dist, start_pos " %f %f %f %f %f %f %f"
, closest_height2, height, movez_r, accel_multiplier); , sc->oid, count, clock_offset, start_pos
, steps, start_sv, accel
, height, closestxy_sd, closest_height2, movez_r);
return ERROR_RET; return ERROR_RET;
} }
return 0; return 0;
@ -627,15 +640,17 @@ stepcompress_push_delta_accel(
int res = step_dist > 0. ? count : -count; int res = step_dist > 0. ? count : -count;
// Calculate each step time // Calculate each step time
clock_offset += 0.5; double inv_accel = 1. / accel;
start_pos += movexy_r*closestxy_d; 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; height += .5 * step_dist;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
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 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 v = safe_sqrt(closest_height2 - relheight*relheight);
double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
v = safe_sqrt(pos * accel_multiplier); v = safe_sqrt(pos * accel_multiplier);