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

View File

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

View File

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

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