Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
klippy/chelper/__init__.py
View File

@ -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

98
klippy/chelper/itersolve.c
View File

@ -5,6 +5,7 @@
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <math.h> // fabs
#include <stddef.h> // offsetof
#include <string.h> // 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)

13
klippy/chelper/itersolve.h
View File

@ -3,6 +3,10 @@
#include <stdint.h> // 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

38
klippy/mcu.py
View File

@ -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):

14
klippy/stepper.py
View File

@ -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)