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

stepcompress: Pass constant velocity and acceleration directly to C code 

Update the C 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 19:54:32 -04:00
parent e4153a536f
commit 1d81bf5596
3 changed files with 32 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
klippy/chelper.py
View File

@ -22,12 +22,10 @@ defs_stepcompress = """
int stepcompress_push(struct stepcompress *sc, double step_clock int stepcompress_push(struct stepcompress *sc, double step_clock
, int32_t sdir); , int32_t sdir);
int32_t stepcompress_push_factor(struct stepcompress *sc int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset
, double steps, double step_offset , double step_offset, double steps, double cruise_sv);
, double clock_offset, double factor); int32_t stepcompress_push_accel(struct stepcompress *sc, double clock_offset
int32_t stepcompress_push_sqrt(struct stepcompress *sc , double step_offset, double steps, double start_sv, double accel);
, double steps, double step_offset
, double clock_offset, double sqrt_offset, double factor);
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 dist, double start_pos
, double inv_velocity, double step_dist, double height , double inv_velocity, double step_dist, double height

26
klippy/mcu.py
View File

@ -29,7 +29,7 @@ class MCU_stepper:
self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
self._commanded_pos = 0 self._commanded_pos = 0
self._step_dist = self._inv_step_dist = 1. self._step_dist = self._inv_step_dist = 1.
self._velocity_factor = self._inv_accel_factor = 0. self._velocity_factor = self._accel_factor = 0.
self._mcu_position_offset = 0 self._mcu_position_offset = 0
self._mcu_freq = self._min_stop_interval = 0. self._mcu_freq = self._min_stop_interval = 0.
self._reset_cmd = self._get_position_cmd = None self._reset_cmd = self._get_position_cmd = None
@ -43,7 +43,7 @@ class MCU_stepper:
def build_config(self): def build_config(self):
self._mcu_freq = self._mcu.get_mcu_freq() self._mcu_freq = self._mcu.get_mcu_freq()
self._velocity_factor = 1. / (self._mcu_freq * self._step_dist) self._velocity_factor = 1. / (self._mcu_freq * self._step_dist)
self._inv_accel_factor = self._mcu_freq**2 * self._step_dist self._accel_factor = 1. / (self._mcu_freq**2 * self._step_dist)
max_error = self._mcu.get_max_stepper_error() max_error = self._mcu.get_max_stepper_error()
min_stop_interval = max(0., self._min_stop_interval - max_error) min_stop_interval = max(0., self._min_stop_interval - max_error)
self._mcu.add_config_cmd( self._mcu.add_config_cmd(
@ -119,29 +119,21 @@ class MCU_stepper:
else: else:
self._commanded_pos -= 1 self._commanded_pos -= 1
def step_const(self, mcu_time, start_pos, dist, cruise_v): def step_const(self, mcu_time, start_pos, dist, cruise_v):
#t = pos/cruise_v
inv_step_dist = self._inv_step_dist inv_step_dist = self._inv_step_dist
step_offset = self._commanded_pos - start_pos * inv_step_dist step_offset = self._commanded_pos - start_pos * inv_step_dist
steps = dist * inv_step_dist count = self._ffi_lib.stepcompress_push_const(
count = self._ffi_lib.stepcompress_push_factor( self._stepqueue, mcu_time * self._mcu_freq, step_offset,
self._stepqueue, steps, step_offset, dist * inv_step_dist, cruise_v * self._velocity_factor)
mcu_time * self._mcu_freq, 1. / (cruise_v * self._velocity_factor))
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_accel(self, mcu_time, start_pos, dist, start_v, accel): def step_accel(self, mcu_time, start_pos, dist, start_v, accel):
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
inv_step_dist = self._inv_step_dist inv_step_dist = self._inv_step_dist
mcu_freq = self._mcu_freq
inv_accel = 1. / accel
time_offset = start_v * inv_accel * mcu_freq
sqrt_offset = time_offset**2
step_offset = self._commanded_pos - start_pos * inv_step_dist step_offset = self._commanded_pos - start_pos * inv_step_dist
steps = dist * inv_step_dist count = self._ffi_lib.stepcompress_push_accel(
clock = mcu_time * mcu_freq - time_offset self._stepqueue, mcu_time * self._mcu_freq, step_offset,
count = self._ffi_lib.stepcompress_push_sqrt( dist * inv_step_dist, start_v * self._velocity_factor,
self._stepqueue, steps, step_offset, clock, accel * self._accel_factor)
sqrt_offset, 2. * inv_accel * self._inv_accel_factor)
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

34
klippy/stepcompress.c
View File

@ -465,11 +465,10 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
} }
// Schedule 'steps' number of steps with a constant time between steps // Schedule 'steps' number of steps with a constant time between steps
// using the formula: step_clock = clock_offset + step_num*factor // using the formula: step_clock = clock_offset + step_num/cruise_sv
int32_t int32_t
stepcompress_push_factor(struct stepcompress *sc stepcompress_push_const(struct stepcompress *sc, double clock_offset
, double steps, double step_offset , double step_offset, double steps, double cruise_sv)
, double clock_offset, double factor)
{ {
// Calculate number of steps to take // Calculate number of steps to take
int sdir = 1; int sdir = 1;
@ -481,8 +480,8 @@ stepcompress_push_factor(struct stepcompress *sc
int count = steps + .5 - step_offset; int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) { if (count <= 0 || count > 10000000) {
if (count && steps) { if (count && steps) {
errorf("push_factor invalid count %d %f %f %f %f" errorf("push_const invalid count %d %f %f %f %f"
, sc->oid, steps, step_offset, clock_offset, factor); , sc->oid, clock_offset, step_offset, steps, cruise_sv);
return ERROR_RET; return ERROR_RET;
} }
return 0; return 0;
@ -494,6 +493,7 @@ stepcompress_push_factor(struct stepcompress *sc
// Calculate each step time // Calculate each step time
clock_offset += 0.5; clock_offset += 0.5;
double factor = 1. / cruise_sv;
double pos = step_offset + .5; double pos = step_offset + .5;
uint64_t *qn = sc->queue_next, *qend = sc->queue_end; uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
while (count--) { while (count--) {
@ -507,11 +507,14 @@ stepcompress_push_factor(struct stepcompress *sc
return res; return res;
} }
// Schedule 'steps' number of steps using the formula: // Schedule 'steps' number of steps at constant acceleration. It uses
// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset) // the formula:
// step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
// - start_sv/accel)
int32_t int32_t
stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset stepcompress_push_accel(struct stepcompress *sc, double clock_offset
, double clock_offset, double sqrt_offset, double factor) , double step_offset, double steps
, double start_sv, double accel)
{ {
// Calculate number of steps to take // Calculate number of steps to take
int sdir = 1; int sdir = 1;
@ -523,9 +526,8 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
int count = steps + .5 - step_offset; int count = steps + .5 - step_offset;
if (count <= 0 || count > 10000000) { if (count <= 0 || count > 10000000) {
if (count && steps) { if (count && steps) {
errorf("push_sqrt invalid count %d %f %f %f %f %f" errorf("push_accel invalid count %d %f %f %f %f %f"
, sc->oid, steps, step_offset, clock_offset, sqrt_offset , sc->oid, clock_offset, step_offset, steps, start_sv, accel);
, factor);
return ERROR_RET; return ERROR_RET;
} }
return 0; return 0;
@ -536,8 +538,10 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
int res = sdir ? count : -count; int res = sdir ? count : -count;
// Calculate each step time // Calculate each step time
clock_offset += 0.5; double inv_accel = 1. / accel;
double pos = step_offset + .5 + sqrt_offset/factor; double factor = 2. * inv_accel;
clock_offset += 0.5 - start_sv * inv_accel;
double pos = step_offset + .5 + .5 * start_sv*start_sv * inv_accel;
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);