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
, double step_offset, double steps, double start_sv, double accel);
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);
, 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
, 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);
, double clock_offset, double start_pos, double steps, double start_sv
, double accel, double height, double closestxy_sd
, double closest_height2, double movez_r);
struct steppersync *steppersync_alloc(struct serialqueue *sq
, 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
def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
, height_base, closestxy_d, closest_height2, movez_r):
mcu_freq = self._mcu_freq
step_dist = self._step_dist
height = self._commanded_pos*step_dist - height_base
if dist < 0:
dist = -dist
step_dist = -step_dist
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 * mcu_freq, dist, -start_pos
, mcu_freq / cruise_v, step_dist
, height, closestxy_d, closest_height2, movez_r)
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
mcu_freq = self._mcu_freq
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
height = self._commanded_pos - height_base * inv_step_dist
count = self._ffi_lib.stepcompress_push_delta_accel(
self._stepqueue, clock, dist, accel_offset - start_pos
, 2. * inv_accel * mcu_freq**2, step_dist
, height, closestxy_d, closest_height2, movez_r)
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,
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

61
klippy/stepcompress.c
View File

@ -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);