stepcompress: Do all step rounding in C code

Commits f0cefebf and 8f331f08 changed the way the code determined what
steps to take on fractional steps.  Unfortunately, it was possible in
some situations for the C code to round differently from the python
code which could result in warnings and lost steps.

Change the code so that all fractional step handling is done in the C
code.  Implementing the step rounding logic in one location avoids any
conflicts.

In order to efficiently handle the step rounding in the C code, the C
code has also been extended to directly send the "set_next_step_dir"
command.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2016-11-10 12:44:04 -05:00
parent 79da35d023
commit 7554c7f694
6 changed files with 154 additions and 131 deletions

View File

@ -97,45 +97,47 @@ class CartKinematics:
inv_accel = 1. / move.accel inv_accel = 1. / move.accel
inv_cruise_v = 1. / move.cruise_v inv_cruise_v = 1. / move.cruise_v
for i in StepList: for i in StepList:
inv_step_dist = self.steppers[i].inv_step_dist if not move.axes_d[i]:
new_step_pos = int(move.end_pos[i]*inv_step_dist + 0.5)
step_pos = self.stepper_pos[i]
if new_step_pos == step_pos:
continue continue
self.stepper_pos[i] = new_step_pos mcu_time, so = self.steppers[i].prep_move(move_time)
inv_step_dist = self.steppers[i].inv_step_dist
steps = move.axes_d[i] * inv_step_dist steps = move.axes_d[i] * inv_step_dist
step_offset = step_pos - move.start_pos[i] * inv_step_dist + 0.5 move_step_d = move.move_d / abs(steps)
sdir = 1
if steps < 0:
sdir = 0
steps = -steps
step_offset = 1. - step_offset
mcu_time, so = self.steppers[i].prep_move(move_time, sdir)
move_step_d = move.move_d / steps step_pos = self.stepper_pos[i]
step_offset = step_pos - move.start_pos[i] * inv_step_dist
# Acceleration steps # Acceleration steps
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = move.start_v * inv_accel
accel_sqrt_offset = accel_time_offset**2
accel_multiplier = 2.0 * move_step_d * inv_accel accel_multiplier = 2.0 * move_step_d * inv_accel
accel_steps = move.accel_r * steps if move.accel_r:
step_offset = so.step_sqrt( #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
mcu_time - accel_time_offset, accel_steps, step_offset accel_time_offset = move.start_v * inv_accel
, accel_sqrt_offset, accel_multiplier) accel_sqrt_offset = accel_time_offset**2
mcu_time += move.accel_t accel_steps = move.accel_r * steps
count = so.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
step_pos += count
step_offset += count - accel_steps
mcu_time += move.accel_t
# Cruising steps # Cruising steps
#t = pos/cruise_v if move.cruise_r:
cruise_multiplier = move_step_d * inv_cruise_v #t = pos/cruise_v
cruise_steps = move.cruise_r * steps cruise_multiplier = move_step_d * inv_cruise_v
step_offset = so.step_factor( cruise_steps = move.cruise_r * steps
mcu_time, cruise_steps, step_offset, cruise_multiplier) count = so.step_factor(
mcu_time += move.cruise_t mcu_time, cruise_steps, step_offset, cruise_multiplier)
step_pos += count
step_offset += count - cruise_steps
mcu_time += move.cruise_t
# Deceleration steps # Deceleration steps
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) if move.decel_r:
decel_time_offset = move.cruise_v * inv_accel #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_sqrt_offset = decel_time_offset**2 decel_time_offset = move.cruise_v * inv_accel
decel_steps = move.decel_r * steps decel_sqrt_offset = decel_time_offset**2
so.step_sqrt( decel_steps = move.decel_r * steps
mcu_time + decel_time_offset, decel_steps, step_offset count = so.step_sqrt(
, decel_sqrt_offset, -accel_multiplier) mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier)
step_pos += count
self.stepper_pos[i] = step_pos

View File

@ -13,12 +13,14 @@ OTHER_FILES = ['list.h', 'serialqueue.h']
defs_stepcompress = """ defs_stepcompress = """
struct stepcompress *stepcompress_alloc(uint32_t max_error struct stepcompress *stepcompress_alloc(uint32_t max_error
, uint32_t queue_step_msgid, uint32_t oid); , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
void stepcompress_push(struct stepcompress *sc, double step_clock); , uint32_t invert_sdir, uint32_t oid);
double stepcompress_push_factor(struct stepcompress *sc void 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 steps, double step_offset
, double clock_offset, double factor); , double clock_offset, double factor);
double stepcompress_push_sqrt(struct stepcompress *sc int32_t stepcompress_push_sqrt(struct stepcompress *sc
, double steps, double step_offset , double steps, double step_offset
, double clock_offset, double sqrt_offset, double factor); , double clock_offset, double sqrt_offset, double factor);
void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);

View File

@ -88,69 +88,58 @@ class PrinterExtruder:
# There is still only a decel phase (no retraction) # There is still only a decel phase (no retraction)
decel_d -= extra_decel_d decel_d -= extra_decel_d
# Determine regular steps # Prepare for steps
forward_d = accel_d + cruise_d + decel_d stepper_pos = self.stepper_pos
end_pos = start_pos + forward_d
inv_step_dist = self.stepper.inv_step_dist inv_step_dist = self.stepper.inv_step_dist
new_step_pos = int(end_pos*inv_step_dist + 0.5) step_dist = self.stepper.step_dist
if new_step_pos != self.stepper_pos: mcu_time, so = self.stepper.prep_move(move_time)
steps = forward_d * inv_step_dist step_offset = stepper_pos - start_pos * inv_step_dist
step_offset = self.stepper_pos - start_pos * inv_step_dist + 0.5
self.stepper_pos = new_step_pos
sdir = 1
if steps < 0:
sdir = 0
steps = -steps
step_offset = 1. - step_offset
mcu_time, so = self.stepper.prep_move(move_time, sdir)
move_step_d = forward_d / steps # Acceleration steps
inv_move_step_d = 1. / move_step_d accel_multiplier = 2.0 * step_dist * inv_accel
if accel_d:
# Acceleration steps
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = start_v * inv_accel accel_time_offset = start_v * inv_accel
accel_sqrt_offset = accel_time_offset**2 accel_sqrt_offset = accel_time_offset**2
accel_multiplier = 2.0 * move_step_d * inv_accel accel_steps = accel_d * inv_step_dist
accel_steps = accel_d * inv_move_step_d count = so.step_sqrt(
step_offset = so.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier) , accel_sqrt_offset, accel_multiplier)
stepper_pos += count
step_offset += count - accel_steps
mcu_time += accel_t mcu_time += accel_t
# Cruising steps # Cruising steps
if cruise_d:
#t = pos/cruise_v #t = pos/cruise_v
cruise_multiplier = move_step_d / cruise_v cruise_multiplier = step_dist / cruise_v
cruise_steps = cruise_d * inv_move_step_d cruise_steps = cruise_d * inv_step_dist
step_offset = so.step_factor( count = so.step_factor(
mcu_time, cruise_steps, step_offset, cruise_multiplier) mcu_time, cruise_steps, step_offset, cruise_multiplier)
stepper_pos += count
step_offset += count - cruise_steps
mcu_time += cruise_t mcu_time += cruise_t
# Deceleration steps # Deceleration steps
if decel_d:
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_time_offset = decel_v * inv_accel decel_time_offset = decel_v * inv_accel
decel_sqrt_offset = decel_time_offset**2 decel_sqrt_offset = decel_time_offset**2
decel_steps = decel_d * inv_move_step_d decel_steps = decel_d * inv_step_dist
so.step_sqrt( count = so.step_sqrt(
mcu_time + decel_time_offset, decel_steps, step_offset mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier) , decel_sqrt_offset, -accel_multiplier)
stepper_pos += count
# Determine retract steps step_offset += count - decel_steps
start_pos = end_pos mcu_time += decel_t
end_pos -= retract_d # Retraction steps
new_step_pos = int(end_pos*inv_step_dist + 0.5) if retract_d:
if new_step_pos != self.stepper_pos:
steps = retract_d * inv_step_dist
step_offset = start_pos * inv_step_dist - self.stepper_pos + 0.5
self.stepper_pos = new_step_pos
mcu_time, so = self.stepper.prep_move(
move_time+accel_t+cruise_t+decel_t, 0)
move_step_d = retract_d / steps
# Acceleration steps
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = retract_v * inv_accel accel_time_offset = retract_v * inv_accel
accel_sqrt_offset = accel_time_offset**2 accel_sqrt_offset = accel_time_offset**2
accel_multiplier = 2.0 * move_step_d * inv_accel accel_steps = -retract_d * inv_step_dist
so.step_sqrt(mcu_time - accel_time_offset, steps, step_offset count = so.step_sqrt(
, accel_sqrt_offset, accel_multiplier) mcu_time - accel_time_offset, accel_steps, step_offset
self.extrude_pos = end_pos , accel_sqrt_offset, accel_multiplier)
stepper_pos += count
self.stepper_pos = stepper_pos
self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d

View File

@ -22,7 +22,7 @@ class MCU_stepper:
self._oid = mcu.create_oid() self._oid = mcu.create_oid()
step_pin, pullup, invert_step = parse_pin_extras(step_pin) step_pin, pullup, invert_step = parse_pin_extras(step_pin)
dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
self._sdir = -1 self._need_reset = True
self._mcu_freq = mcu.get_mcu_freq() self._mcu_freq = mcu.get_mcu_freq()
min_stop_interval = int(min_stop_interval * self._mcu_freq) min_stop_interval = int(min_stop_interval * self._mcu_freq)
max_error = int(max_error * self._mcu_freq) max_error = int(max_error * self._mcu_freq)
@ -39,30 +39,26 @@ class MCU_stepper:
"reset_step_clock oid=%c clock=%u") "reset_step_clock oid=%c clock=%u")
ffi_main, self.ffi_lib = chelper.get_ffi() ffi_main, self.ffi_lib = chelper.get_ffi()
self._stepqueue = self.ffi_lib.stepcompress_alloc( self._stepqueue = self.ffi_lib.stepcompress_alloc(
max_error, self._step_cmd.msgid, self._oid) max_error, self._step_cmd.msgid
, self._dir_cmd.msgid, self._invert_dir, self._oid)
self.print_to_mcu_time = mcu.print_to_mcu_time self.print_to_mcu_time = mcu.print_to_mcu_time
def get_oid(self): def get_oid(self):
return self._oid return self._oid
def get_invert_dir(self): def get_invert_dir(self):
return self._invert_dir return self._invert_dir
def note_stepper_stop(self): def note_stepper_stop(self):
self._sdir = -1 self._need_reset = True
def _reset_step_clock(self, clock): def check_reset(self, mcu_time):
if not self._need_reset:
return
self._need_reset = False
clock = int(mcu_time * self._mcu_freq)
self.ffi_lib.stepcompress_reset(self._stepqueue, clock) self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff) data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data)) self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
def set_next_step_dir(self, mcu_time, sdir): def step(self, mcu_time, sdir):
if self._sdir == sdir:
return
if self._sdir == -1:
clock = int(mcu_time * self._mcu_freq)
self._reset_step_clock(clock)
self._sdir = sdir
data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir)
self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
def step(self, mcu_time):
clock = mcu_time * self._mcu_freq clock = mcu_time * self._mcu_freq
self.ffi_lib.stepcompress_push(self._stepqueue, clock) self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor): def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
clock = mcu_time * self._mcu_freq clock = mcu_time * self._mcu_freq
mcu_freq2 = self._mcu_freq**2 mcu_freq2 = self._mcu_freq**2
@ -477,7 +473,7 @@ class Dummy_MCU_stepper:
self._stepid, dirstr, countstr, addstr, interval)) self._stepid, dirstr, countstr, addstr, interval))
def set_next_step_dir(self, dir): def set_next_step_dir(self, dir):
self._sdir = dir self._sdir = dir
def _reset_step_clock(self, clock): def check_reset(self, clock):
self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock)) self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
def print_to_mcu_time(self, print_time): def print_to_mcu_time(self, print_time):
return self._mcu.print_to_mcu_time(print_time) return self._mcu.print_to_mcu_time(print_time)

View File

@ -35,7 +35,8 @@ struct stepcompress {
// Message generation // Message generation
uint64_t last_step_clock; uint64_t last_step_clock;
struct list_head msg_queue; struct list_head msg_queue;
uint32_t queue_step_msgid, oid; uint32_t queue_step_msgid, set_next_step_dir_msgid, oid;
int sdir, invert_sdir;
}; };
@ -268,14 +269,19 @@ safe_sqrt(double v)
// Allocate a new 'stepcompress' object // Allocate a new 'stepcompress' object
struct stepcompress * struct stepcompress *
stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid, uint32_t oid) stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid
, uint32_t set_next_step_dir_msgid, uint32_t invert_sdir
, uint32_t oid)
{ {
struct stepcompress *sc = malloc(sizeof(*sc)); struct stepcompress *sc = malloc(sizeof(*sc));
memset(sc, 0, sizeof(*sc)); memset(sc, 0, sizeof(*sc));
sc->max_error = max_error; sc->max_error = max_error;
list_init(&sc->msg_queue); list_init(&sc->msg_queue);
sc->queue_step_msgid = queue_step_msgid; sc->queue_step_msgid = queue_step_msgid;
sc->set_next_step_dir_msgid = set_next_step_dir_msgid;
sc->oid = oid; sc->oid = oid;
sc->sdir = -1;
sc->invert_sdir = !!invert_sdir;
return sc; return sc;
} }
@ -312,75 +318,102 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
} }
} }
// Send the set_next_step_dir command
static void
set_next_step_dir(struct stepcompress *sc, int sdir)
{
sc->sdir = sdir;
stepcompress_flush(sc, UINT64_MAX);
uint32_t msg[3] = {
sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
};
struct queue_message *qm = message_alloc_and_encode(msg, 3);
qm->req_clock = sc->last_step_clock;
list_add_tail(&qm->node, &sc->msg_queue);
}
// Check if the internal queue needs to be expanded, and expand if so // Check if the internal queue needs to be expanded, and expand if so
static inline void static inline void
check_expand(struct stepcompress *sc, int count) check_expand(struct stepcompress *sc, int sdir, int count)
{ {
if (sdir != sc->sdir)
set_next_step_dir(sc, sdir);
if (sc->queue_next + count > sc->queue_end) if (sc->queue_next + count > sc->queue_end)
expand_queue(sc, count); expand_queue(sc, count);
} }
// Schedule a step event at the specified step_clock time // Schedule a step event at the specified step_clock time
void void
stepcompress_push(struct stepcompress *sc, double step_clock) stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
{ {
check_expand(sc, 1); sdir = !!sdir;
check_expand(sc, sdir, 1);
step_clock += 0.5; step_clock += 0.5;
*sc->queue_next++ = step_clock; *sc->queue_next++ = step_clock;
} }
// 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*factor
double int32_t
stepcompress_push_factor(struct stepcompress *sc stepcompress_push_factor(struct stepcompress *sc
, double steps, double step_offset , double steps, double step_offset
, double clock_offset, double factor) , double clock_offset, double factor)
{ {
// Calculate number of steps to take // Calculate number of steps to take
double ceil_steps = ceil(steps - step_offset); int sdir = 1;
double next_step_offset = ceil_steps - (steps - step_offset); if (steps < 0) {
int count = ceil_steps; sdir = 0;
if (count < 0 || count > 1000000) { steps = -steps;
fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n" step_offset = -step_offset;
, sc->oid, steps, step_offset, clock_offset, factor);
return next_step_offset;
} }
check_expand(sc, count); int count = steps + .5 - step_offset;
if (count <= 0 || count > 1000000) {
if (count && steps)
fprintf(stderr, "ERROR: push_factor invalid count %d %f %f %f %f\n"
, sc->oid, steps, step_offset, clock_offset, factor);
return 0;
}
check_expand(sc, sdir, count);
// Calculate each step time // Calculate each step time
uint64_t *qn = sc->queue_next, *end = &qn[count]; uint64_t *qn = sc->queue_next, *end = &qn[count];
clock_offset += 0.5; clock_offset += 0.5;
double pos = step_offset; double pos = step_offset + .5;
while (qn < end) { while (qn < end) {
*qn++ = clock_offset + pos*factor; *qn++ = clock_offset + pos*factor;
pos += 1.0; pos += 1.0;
} }
sc->queue_next = qn; sc->queue_next = qn;
return next_step_offset; return sdir ? count : -count;
} }
// Schedule 'steps' number of steps using the formula: // Schedule 'steps' number of steps using the formula:
// step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset) // step_clock = clock_offset + sqrt(step_num*factor + sqrt_offset)
double int32_t
stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
, double clock_offset, double sqrt_offset, double factor) , double clock_offset, double sqrt_offset, double factor)
{ {
// Calculate number of steps to take // Calculate number of steps to take
double ceil_steps = ceil(steps - step_offset); int sdir = 1;
double next_step_offset = ceil_steps - (steps - step_offset); if (steps < 0) {
int count = ceil_steps; sdir = 0;
if (count < 0 || count > 1000000) { steps = -steps;
fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n" step_offset = -step_offset;
, sc->oid, steps, step_offset, clock_offset, sqrt_offset
, factor);
return next_step_offset;
} }
check_expand(sc, count); int count = steps + .5 - step_offset;
if (count <= 0 || count > 1000000) {
if (count && steps)
fprintf(stderr, "ERROR: push_sqrt invalid count %d %f %f %f %f %f\n"
, sc->oid, steps, step_offset, clock_offset, sqrt_offset
, factor);
return 0;
}
check_expand(sc, sdir, count);
// Calculate each step time // Calculate each step time
uint64_t *qn = sc->queue_next, *end = &qn[count]; uint64_t *qn = sc->queue_next, *end = &qn[count];
clock_offset += 0.5; clock_offset += 0.5;
double pos = step_offset + sqrt_offset/factor; double pos = step_offset + .5 + sqrt_offset/factor;
if (factor >= 0.0) if (factor >= 0.0)
while (qn < end) { while (qn < end) {
*qn++ = clock_offset + safe_sqrt(pos*factor); *qn++ = clock_offset + safe_sqrt(pos*factor);
@ -392,7 +425,7 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
pos += 1.0; pos += 1.0;
} }
sc->queue_next = qn; sc->queue_next = qn;
return next_step_offset; return sdir ? count : -count;
} }
// Reset the internal state of the stepcompress object // Reset the internal state of the stepcompress object
@ -401,6 +434,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
{ {
stepcompress_flush(sc, UINT64_MAX); stepcompress_flush(sc, UINT64_MAX);
sc->last_step_clock = last_step_clock; sc->last_step_clock = last_step_clock;
sc->sdir = -1;
} }
// Queue an mcu command to go out in order with stepper commands // Queue an mcu command to go out in order with stepper commands

View File

@ -69,9 +69,9 @@ class PrinterStepper:
mcu_time = self.mcu_enable.print_to_mcu_time(move_time) mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
self.mcu_enable.set_digital(mcu_time, enable) self.mcu_enable.set_digital(mcu_time, enable)
self.need_motor_enable = True self.need_motor_enable = True
def prep_move(self, move_time, sdir): def prep_move(self, move_time):
mcu_time = self.mcu_stepper.print_to_mcu_time(move_time) mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
self.mcu_stepper.set_next_step_dir(mcu_time, sdir) self.mcu_stepper.check_reset(mcu_time)
if self.need_motor_enable: if self.need_motor_enable:
self.motor_enable(move_time, 1) self.motor_enable(move_time, 1)
self.need_motor_enable = False self.need_motor_enable = False