From f3ef9c18899e06eb66301fe8c994bcc67955905b Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 27 Oct 2019 21:05:57 -0400 Subject: [PATCH] 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 --- klippy/chelper/__init__.py | 5 ++ klippy/chelper/itersolve.c | 98 +++++++++++++++++++++++++++++++++++--- klippy/chelper/itersolve.h | 13 +++++ klippy/mcu.py | 38 +++++++++++++-- klippy/stepper.py | 14 ++++++ 5 files changed, 158 insertions(+), 10 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 91929747..1d980a77 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -45,6 +45,11 @@ defs_stepcompress = """ defs_itersolve = """ 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 , struct stepcompress *sc, double step_dist); double itersolve_calc_position_from_coord(struct stepper_kinematics *sk diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 3f35f1d4..c84d25cc 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -5,6 +5,7 @@ // This file may be distributed under the terms of the GNU GPLv3 license. #include // fabs +#include // offsetof #include // memset #include "compiler.h" // __visible #include "itersolve.h" // itersolve_gen_steps @@ -53,15 +54,17 @@ itersolve_find_step(struct stepper_kinematics *sk, struct move *m return best_guess; } -// Generate step times for a stepper during a move -int32_t __visible -itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) +// Generate step times for a portion of a move +static int32_t +itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m + , double move_start, double move_end) { struct stepcompress *sc = sk->sc; sk_calc_callback calc_position_cb = sk->calc_position_cb; double half_step = .5 * sk->step_dist; 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; int sdir = stepcompress_get_step_dir(sc); 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; if (fabs(dist) < half_step) { seek_new_high_range: - if (high.time >= m->move_t) + if (high.time >= end) // At end of move break; // Need to increase next step search range low = high; high.time = last.time + seek_time_delta; seek_time_delta += seek_time_delta; - if (high.time > m->move_t) - high.time = m->move_t; + if (high.time > end) + high.time = end; high.position = calc_position_cb(sk, m, high.time); continue; } @@ -125,6 +128,87 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) 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 itersolve_set_stepcompress(struct stepper_kinematics *sk , struct stepcompress *sc, double step_dist) diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h index da3a9fa7..e7548849 100644 --- a/klippy/chelper/itersolve.h +++ b/klippy/chelper/itersolve.h @@ -3,6 +3,10 @@ #include // int32_t +enum { + AF_X = 1 << 0, AF_Y = 1 << 1, AF_Z = 1 <<2, +}; + struct stepper_kinematics; struct move; 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 { double step_dist, commanded_pos; struct stepcompress *sc; + + double last_flush_time; + struct trapq *tq; + int active_flags; + sk_calc_callback calc_position_cb; sk_post_callback post_cb; }; 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 , struct stepcompress *sc, double step_dist); double itersolve_calc_position_from_coord(struct stepper_kinematics *sk diff --git a/klippy/mcu.py b/klippy/mcu.py index 9864474d..ec490e65 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -21,11 +21,14 @@ class MCU_stepper: self._step_dist = 0. self._min_stop_interval = 0. self._reset_cmd_id = self._get_position_cmd = None + self._active_callbacks = [] ffi_main, self._ffi_lib = chelper.get_ffi() self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid), self._ffi_lib.stepcompress_free) self._mcu.register_stepqueue(self._stepqueue) 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) def get_mcu(self): return self._mcu @@ -95,12 +98,16 @@ class MCU_stepper: sk, self._stepqueue, self._step_dist) return old_sk def set_ignore_move(self, ignore_move): - was_ignore = (self._itersolve_gen_steps - is not self._ffi_lib.itersolve_gen_steps) + fl = self._ffi_lib + was_ignore = self._itersolve_gen_steps is not fl.itersolve_gen_steps if ignore_move: self._itersolve_gen_steps = (lambda *args: 0) + self._itersolve_generate_steps = (lambda *args: 0) + self._itersolve_check_active = (lambda *args: 0.) 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 def note_homing_end(self, did_trigger=False): 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) if ret: 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 TimeoutError(Exception): diff --git a/klippy/stepper.py b/klippy/stepper.py index 4617f97a..f3721b45 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -70,6 +70,7 @@ class PrinterStepper: mcu_stepper.setup_dir_pin(dir_pin_params) step_dist = config.getfloat('step_distance', above=0.) 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)) # Register STEPPER_BUZZ command force_move = printer.try_load_module(config, 'force_move') @@ -77,6 +78,8 @@ class PrinterStepper: # Wrappers self.step_itersolve = mcu_stepper.step_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_ignore_move = mcu_stepper.set_ignore_move 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) min_stop_interval = second_last_step_time - last_step_time 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): if self.need_motor_enable != (not enable): self.enable.set_enable(print_time, 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): return not self.need_motor_enable @@ -213,6 +221,12 @@ class PrinterRail: def setup_itersolve(self, alloc_func, *params): for stepper in self.steppers: 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): for stepper in self.steppers: stepper.set_max_jerk(max_halt_velocity, max_accel)