itersolve: Add support for generating steps from a trapq

Support associating a stepper_kinematics with a trapq.  Support
generating steps from a time range on the given trapq.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2019-10-27 21:05:57 -04:00
parent d3afe4f1d8
commit f3ef9c1889
5 changed files with 158 additions and 10 deletions

View File

@ -45,6 +45,11 @@ defs_stepcompress = """
defs_itersolve = """ defs_itersolve = """
int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m); int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
int32_t itersolve_generate_steps(struct stepper_kinematics *sk
, double flush_time);
double itersolve_check_active(struct stepper_kinematics *sk
, double flush_time);
void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
void itersolve_set_stepcompress(struct stepper_kinematics *sk void itersolve_set_stepcompress(struct stepper_kinematics *sk
, struct stepcompress *sc, double step_dist); , struct stepcompress *sc, double step_dist);
double itersolve_calc_position_from_coord(struct stepper_kinematics *sk double itersolve_calc_position_from_coord(struct stepper_kinematics *sk

View File

@ -5,6 +5,7 @@
// This file may be distributed under the terms of the GNU GPLv3 license. // This file may be distributed under the terms of the GNU GPLv3 license.
#include <math.h> // fabs #include <math.h> // fabs
#include <stddef.h> // offsetof
#include <string.h> // memset #include <string.h> // memset
#include "compiler.h" // __visible #include "compiler.h" // __visible
#include "itersolve.h" // itersolve_gen_steps #include "itersolve.h" // itersolve_gen_steps
@ -53,15 +54,17 @@ itersolve_find_step(struct stepper_kinematics *sk, struct move *m
return best_guess; return best_guess;
} }
// Generate step times for a stepper during a move // Generate step times for a portion of a move
int32_t __visible static int32_t
itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
, double move_start, double move_end)
{ {
struct stepcompress *sc = sk->sc; struct stepcompress *sc = sk->sc;
sk_calc_callback calc_position_cb = sk->calc_position_cb; sk_calc_callback calc_position_cb = sk->calc_position_cb;
double half_step = .5 * sk->step_dist; double half_step = .5 * sk->step_dist;
double mcu_freq = stepcompress_get_mcu_freq(sc); double mcu_freq = stepcompress_get_mcu_freq(sc);
struct timepos last = { 0., sk->commanded_pos }, low = last, high = last; double start = move_start - m->print_time, end = move_end - m->print_time;
struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
double seek_time_delta = 0.000100; double seek_time_delta = 0.000100;
int sdir = stepcompress_get_step_dir(sc); int sdir = stepcompress_get_step_dir(sc);
struct queue_append qa = queue_append_start(sc, m->print_time, .5); struct queue_append qa = queue_append_start(sc, m->print_time, .5);
@ -70,15 +73,15 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
double dist = high.position - last.position; double dist = high.position - last.position;
if (fabs(dist) < half_step) { if (fabs(dist) < half_step) {
seek_new_high_range: seek_new_high_range:
if (high.time >= m->move_t) if (high.time >= end)
// At end of move // At end of move
break; break;
// Need to increase next step search range // Need to increase next step search range
low = high; low = high;
high.time = last.time + seek_time_delta; high.time = last.time + seek_time_delta;
seek_time_delta += seek_time_delta; seek_time_delta += seek_time_delta;
if (high.time > m->move_t) if (high.time > end)
high.time = m->move_t; high.time = end;
high.position = calc_position_cb(sk, m, high.time); high.position = calc_position_cb(sk, m, high.time);
continue; continue;
} }
@ -125,6 +128,87 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
return 0; return 0;
} }
// Generate step times for a move
int32_t __visible
itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
{
return itersolve_gen_steps_range(sk, m, m->print_time
, m->print_time + m->move_t);
}
// Check if a move is likely to cause movement on a stepper
static inline int
check_active(struct stepper_kinematics *sk, struct move *m)
{
int af = sk->active_flags;
return ((af & AF_X && m->axes_r.x != 0.)
|| (af & AF_Y && m->axes_r.y != 0.)
|| (af & AF_Z && m->axes_r.z != 0.));
}
// Generate step times for a range of moves on the trapq
int32_t __visible
itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
{
double last_flush_time = sk->last_flush_time;
sk->last_flush_time = flush_time;
if (!sk->tq || list_empty(&sk->tq->moves))
return 0;
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
for (;;) {
double move_print_time = m->print_time;
double move_end_time = move_print_time + m->move_t;
if (last_flush_time >= move_end_time) {
if (list_is_last(&m->node, &sk->tq->moves))
break;
m = list_next_entry(m, node);
continue;
}
double start = move_print_time, end = move_end_time;
if (start < last_flush_time)
start = last_flush_time;
if (start >= flush_time)
break;
if (end > flush_time)
end = flush_time;
if (check_active(sk, m)) {
int32_t ret = itersolve_gen_steps_range(sk, m, start, end);
if (ret)
return ret;
}
last_flush_time = end;
}
return 0;
}
// Check if the given stepper is likely to be active in the given time range
double __visible
itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
{
if (!sk->tq || list_empty(&sk->tq->moves))
return 0.;
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
while (sk->last_flush_time >= m->print_time + m->move_t) {
if (list_is_last(&m->node, &sk->tq->moves))
return 0.;
m = list_next_entry(m, node);
}
while (m->print_time < flush_time) {
if (check_active(sk, m))
return m->print_time;
if (list_is_last(&m->node, &sk->tq->moves))
return 0.;
m = list_next_entry(m, node);
}
return 0.;
}
void __visible
itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq)
{
sk->tq = tq;
}
void __visible void __visible
itersolve_set_stepcompress(struct stepper_kinematics *sk itersolve_set_stepcompress(struct stepper_kinematics *sk
, struct stepcompress *sc, double step_dist) , struct stepcompress *sc, double step_dist)

View File

@ -3,6 +3,10 @@
#include <stdint.h> // int32_t #include <stdint.h> // int32_t
enum {
AF_X = 1 << 0, AF_Y = 1 << 1, AF_Z = 1 <<2,
};
struct stepper_kinematics; struct stepper_kinematics;
struct move; struct move;
typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
@ -11,11 +15,20 @@ typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
struct stepper_kinematics { struct stepper_kinematics {
double step_dist, commanded_pos; double step_dist, commanded_pos;
struct stepcompress *sc; struct stepcompress *sc;
double last_flush_time;
struct trapq *tq;
int active_flags;
sk_calc_callback calc_position_cb; sk_calc_callback calc_position_cb;
sk_post_callback post_cb; sk_post_callback post_cb;
}; };
int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m); int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
int32_t itersolve_generate_steps(struct stepper_kinematics *sk
, double flush_time);
double itersolve_check_active(struct stepper_kinematics *sk, double flush_time);
void itersolve_set_trapq(struct stepper_kinematics *sk, struct trapq *tq);
void itersolve_set_stepcompress(struct stepper_kinematics *sk void itersolve_set_stepcompress(struct stepper_kinematics *sk
, struct stepcompress *sc, double step_dist); , struct stepcompress *sc, double step_dist);
double itersolve_calc_position_from_coord(struct stepper_kinematics *sk double itersolve_calc_position_from_coord(struct stepper_kinematics *sk

View File

@ -21,11 +21,14 @@ class MCU_stepper:
self._step_dist = 0. self._step_dist = 0.
self._min_stop_interval = 0. self._min_stop_interval = 0.
self._reset_cmd_id = self._get_position_cmd = None self._reset_cmd_id = self._get_position_cmd = None
self._active_callbacks = []
ffi_main, self._ffi_lib = chelper.get_ffi() ffi_main, self._ffi_lib = chelper.get_ffi()
self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid), self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid),
self._ffi_lib.stepcompress_free) self._ffi_lib.stepcompress_free)
self._mcu.register_stepqueue(self._stepqueue) self._mcu.register_stepqueue(self._stepqueue)
self._stepper_kinematics = self._itersolve_gen_steps = None self._stepper_kinematics = self._itersolve_gen_steps = None
self._itersolve_generate_steps = self._itersolve_check_active = None
self._trapq = ffi_main.NULL
self.set_ignore_move(False) self.set_ignore_move(False)
def get_mcu(self): def get_mcu(self):
return self._mcu return self._mcu
@ -95,12 +98,16 @@ class MCU_stepper:
sk, self._stepqueue, self._step_dist) sk, self._stepqueue, self._step_dist)
return old_sk return old_sk
def set_ignore_move(self, ignore_move): def set_ignore_move(self, ignore_move):
was_ignore = (self._itersolve_gen_steps fl = self._ffi_lib
is not self._ffi_lib.itersolve_gen_steps) was_ignore = self._itersolve_gen_steps is not fl.itersolve_gen_steps
if ignore_move: if ignore_move:
self._itersolve_gen_steps = (lambda *args: 0) self._itersolve_gen_steps = (lambda *args: 0)
self._itersolve_generate_steps = (lambda *args: 0)
self._itersolve_check_active = (lambda *args: 0.)
else: else:
self._itersolve_gen_steps = self._ffi_lib.itersolve_gen_steps self._itersolve_gen_steps = fl.itersolve_gen_steps
self._itersolve_generate_steps = fl.itersolve_generate_steps
self._itersolve_check_active = fl.itersolve_check_active
return was_ignore return was_ignore
def note_homing_end(self, did_trigger=False): def note_homing_end(self, did_trigger=False):
ret = self._ffi_lib.stepcompress_reset(self._stepqueue, 0) ret = self._ffi_lib.stepcompress_reset(self._stepqueue, 0)
@ -124,6 +131,31 @@ class MCU_stepper:
ret = self._itersolve_gen_steps(self._stepper_kinematics, cmove) ret = self._itersolve_gen_steps(self._stepper_kinematics, cmove)
if ret: if ret:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
def set_trapq(self, tq):
if tq is None:
ffi_main, self._ffi_lib = chelper.get_ffi()
tq = ffi_main.NULL
self._ffi_lib.itersolve_set_trapq(self._stepper_kinematics, tq)
old_tq = self._trapq
self._trapq = tq
return old_tq
def add_active_callback(self, cb):
self._active_callbacks.append(cb)
def generate_steps(self, flush_time):
# Check for activity if necessary
if self._active_callbacks:
ret = self._itersolve_check_active(self._stepper_kinematics,
flush_time)
if ret:
cbs = self._active_callbacks
self._active_callbacks = []
for cb in cbs:
cb(ret)
# Generate steps
ret = self._itersolve_generate_steps(self._stepper_kinematics,
flush_time)
if ret:
raise error("Internal error in stepcompress")
class MCU_endstop: class MCU_endstop:
class TimeoutError(Exception): class TimeoutError(Exception):

View File

@ -70,6 +70,7 @@ class PrinterStepper:
mcu_stepper.setup_dir_pin(dir_pin_params) mcu_stepper.setup_dir_pin(dir_pin_params)
step_dist = config.getfloat('step_distance', above=0.) step_dist = config.getfloat('step_distance', above=0.)
mcu_stepper.setup_step_distance(step_dist) mcu_stepper.setup_step_distance(step_dist)
mcu_stepper.add_active_callback(self._stepper_active)
self.enable = lookup_enable_pin(ppins, config.get('enable_pin', None)) self.enable = lookup_enable_pin(ppins, config.get('enable_pin', None))
# Register STEPPER_BUZZ command # Register STEPPER_BUZZ command
force_move = printer.try_load_module(config, 'force_move') force_move = printer.try_load_module(config, 'force_move')
@ -77,6 +78,8 @@ class PrinterStepper:
# Wrappers # Wrappers
self.step_itersolve = mcu_stepper.step_itersolve self.step_itersolve = mcu_stepper.step_itersolve
self.setup_itersolve = mcu_stepper.setup_itersolve self.setup_itersolve = mcu_stepper.setup_itersolve
self.generate_steps = mcu_stepper.generate_steps
self.set_trapq = mcu_stepper.set_trapq
self.set_stepper_kinematics = mcu_stepper.set_stepper_kinematics self.set_stepper_kinematics = mcu_stepper.set_stepper_kinematics
self.set_ignore_move = mcu_stepper.set_ignore_move self.set_ignore_move = mcu_stepper.set_ignore_move
self.calc_position_from_coord = mcu_stepper.calc_position_from_coord self.calc_position_from_coord = mcu_stepper.calc_position_from_coord
@ -105,10 +108,15 @@ class PrinterStepper:
2. * step_dist, max_halt_velocity, max_accel) 2. * step_dist, max_halt_velocity, max_accel)
min_stop_interval = second_last_step_time - last_step_time min_stop_interval = second_last_step_time - last_step_time
self.mcu_stepper.setup_min_stop_interval(min_stop_interval) self.mcu_stepper.setup_min_stop_interval(min_stop_interval)
def _stepper_active(self, active_time):
self.motor_enable(active_time, 1)
def motor_enable(self, print_time, enable=0): def motor_enable(self, print_time, enable=0):
if self.need_motor_enable != (not enable): if self.need_motor_enable != (not enable):
self.enable.set_enable(print_time, enable) self.enable.set_enable(print_time, enable)
self.need_motor_enable = not enable self.need_motor_enable = not enable
if not enable:
# Enable stepper on future stepper movement
self.mcu_stepper.add_active_callback(self._stepper_active)
def is_motor_enabled(self): def is_motor_enabled(self):
return not self.need_motor_enable return not self.need_motor_enable
@ -213,6 +221,12 @@ class PrinterRail:
def setup_itersolve(self, alloc_func, *params): def setup_itersolve(self, alloc_func, *params):
for stepper in self.steppers: for stepper in self.steppers:
stepper.setup_itersolve(alloc_func, *params) stepper.setup_itersolve(alloc_func, *params)
def generate_steps(self, flush_time):
for stepper in self.steppers:
stepper.generate_steps(flush_time)
def set_trapq(self, trapq):
for stepper in self.steppers:
stepper.set_trapq(trapq)
def set_max_jerk(self, max_halt_velocity, max_accel): def set_max_jerk(self, max_halt_velocity, max_accel):
for stepper in self.steppers: for stepper in self.steppers:
stepper.set_max_jerk(max_halt_velocity, max_accel) stepper.set_max_jerk(max_halt_velocity, max_accel)