itersolve: Implement a step+dir+step filter

Some stepper motor drivers do not respond well to rapid "step +
direction change + step" events.  In particular, it is believed this
can cause "over current" events on the tmc2208 drivers when they are
in "stealthchop" mode.  Detect these events and remove them from the
generated step times.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2020-02-25 12:54:55 -05:00
parent 8f8c1e2c58
commit c9cb462f90
3 changed files with 85 additions and 8 deletions

View File

@ -13,6 +13,65 @@
#include "stepcompress.h" // queue_append_start #include "stepcompress.h" // queue_append_start
#include "trapq.h" // struct move #include "trapq.h" // struct move
/****************************************************************
* Filter rapid "step + direction change + step" sequences
****************************************************************/
#define SDS_CHECK_TIME .001
#define SDS_FILTER_TIME .000750
static int
sds_commit(struct stepper_kinematics *sk)
{
double mtime = sk->next_move_print_time, stime = sk->next_step_time;
sk->next_move_print_time = 0.;
return stepcompress_append(sk->sc, sk->next_step_dir, mtime, stime);
}
static int
sds_append(struct stepper_kinematics *sk, int sdir
, double move_print_time, double step_time)
{
if (sk->next_move_print_time) {
if (sdir != sk->next_step_dir) {
double mt_diff = move_print_time - sk->next_move_print_time;
double st_diff = step_time - sk->next_step_time;
if (mt_diff + st_diff < SDS_FILTER_TIME) {
// Rollback last step
sk->next_move_print_time = 0.;
sk->next_step_dir = sdir;
return 0;
}
}
int ret = sds_commit(sk);
if (ret)
return ret;
}
sk->next_move_print_time = move_print_time;
sk->next_step_time = step_time;
sk->next_step_dir = sdir;
return 0;
}
static int
sds_flush(struct stepper_kinematics *sk
, double move_print_time, double step_time)
{
if (sk->next_move_print_time) {
double mt_diff = move_print_time - sk->next_move_print_time;
double st_diff = step_time - sk->next_step_time;
if (mt_diff + st_diff >= SDS_FILTER_TIME)
return sds_commit(sk);
}
return 0;
}
/****************************************************************
* Main iterative solver
****************************************************************/
struct timepos { struct timepos {
double time, position; double time, position;
}; };
@ -66,7 +125,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
double start = move_start - m->print_time, end = move_end - m->print_time; double start = move_start - m->print_time, end = move_end - m->print_time;
struct timepos last = { start, sk->commanded_pos }, low = last, high = last; struct timepos last = { start, sk->commanded_pos }, low = last, high = last;
double seek_time_delta = SEEK_TIME_RESET; double seek_time_delta = SEEK_TIME_RESET;
int sdir = !!stepcompress_get_step_dir(sk->sc), is_dir_change = 0; int sdir = sk->next_step_dir, is_dir_change = 0;
for (;;) { for (;;) {
double diff = high.position - last.position, dist = sdir ? diff : -diff; double diff = high.position - last.position, dist = sdir ? diff : -diff;
if (dist >= half_step) { if (dist >= half_step) {
@ -74,8 +133,7 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
double target = last.position + (sdir ? half_step : -half_step); double target = last.position + (sdir ? half_step : -half_step);
struct timepos next = itersolve_find_step(sk, m, low, high, target); struct timepos next = itersolve_find_step(sk, m, low, high, target);
// Add step at given time // Add step at given time
int ret = stepcompress_append(sk->sc, sdir int ret = sds_append(sk, sdir, m->print_time, next.time);
, m->print_time, next.time);
if (ret) if (ret)
return ret; return ret;
seek_time_delta = next.time - last.time; seek_time_delta = next.time - last.time;
@ -90,6 +148,10 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
if (low.time < high.time) if (low.time < high.time)
// The existing search range is still valid // The existing search range is still valid
continue; continue;
} else if (dist > 0.) {
// Avoid rollback if stepper fully reaches target position
if (sk->next_move_print_time)
sds_commit(sk);
} else if (unlikely(dist < -(half_step + .000000001))) { } else if (unlikely(dist < -(half_step + .000000001))) {
// Found direction change // Found direction change
is_dir_change = 1; is_dir_change = 1;
@ -121,12 +183,20 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m
high.time = end; high.time = end;
high.position = calc_position_cb(sk, m, high.time); high.position = calc_position_cb(sk, m, high.time);
} }
int ret = sds_flush(sk, m->print_time, end);
if (ret)
return ret;
sk->commanded_pos = last.position; sk->commanded_pos = last.position;
if (sk->post_cb) if (sk->post_cb)
sk->post_cb(sk); sk->post_cb(sk);
return 0; return 0;
} }
/****************************************************************
* Interface functions
****************************************************************/
// Check if a move is likely to cause movement on a stepper // Check if a move is likely to cause movement on a stepper
static inline int static inline int
check_active(struct stepper_kinematics *sk, struct move *m) check_active(struct stepper_kinematics *sk, struct move *m)
@ -149,7 +219,10 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
struct move *m = list_first_entry(&sk->tq->moves, struct move, node); struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
while (last_flush_time >= m->print_time + m->move_t) while (last_flush_time >= m->print_time + m->move_t)
m = list_next_entry(m, node); m = list_next_entry(m, node);
double force_steps_time = sk->last_move_time + sk->gen_steps_post_active; double gen_steps_post_active = sk->gen_steps_post_active;
if (gen_steps_post_active < SDS_CHECK_TIME)
gen_steps_post_active = SDS_CHECK_TIME;
double force_steps_time = sk->last_move_time + gen_steps_post_active;
for (;;) { for (;;) {
if (last_flush_time >= flush_time) if (last_flush_time >= flush_time)
return 0; return 0;
@ -174,7 +247,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
if (ret) if (ret)
return ret; return ret;
sk->last_move_time = last_flush_time = end; sk->last_move_time = last_flush_time = end;
force_steps_time = end + sk->gen_steps_post_active; force_steps_time = end + gen_steps_post_active;
} else if (start < force_steps_time) { } else if (start < force_steps_time) {
// Must generates steps just past stepper activity // Must generates steps just past stepper activity
if (end > force_steps_time) if (end > force_steps_time)

View File

@ -16,6 +16,9 @@ struct stepper_kinematics {
double step_dist, commanded_pos; double step_dist, commanded_pos;
struct stepcompress *sc; struct stepcompress *sc;
double next_move_print_time, next_step_time;
int next_step_dir;
double last_flush_time, last_move_time; double last_flush_time, last_move_time;
struct trapq *tq; struct trapq *tq;
int active_flags; int active_flags;

View File

@ -1,6 +1,6 @@
# Code for coordinating events on the printer toolhead # Code for coordinating events on the printer toolhead
# #
# Copyright (C) 2016-2019 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2016-2020 Kevin O'Connor <kevin@koconnor.net>
# #
# 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.
import math, logging, importlib import math, logging, importlib
@ -183,6 +183,7 @@ class MoveQueue:
MIN_KIN_TIME = 0.100 MIN_KIN_TIME = 0.100
MOVE_BATCH_TIME = 0.500 MOVE_BATCH_TIME = 0.500
SDS_CHECK_TIME = 0.001 # step+dir+step filter in itersolve.c
DRIP_SEGMENT_TIME = 0.050 DRIP_SEGMENT_TIME = 0.050
DRIP_TIME = 0.100 DRIP_TIME = 0.100
@ -236,7 +237,7 @@ class ToolHead:
self.print_stall = 0 self.print_stall = 0
self.drip_completion = None self.drip_completion = None
# Kinematic step generation scan window time tracking # Kinematic step generation scan window time tracking
self.kin_flush_delay = 0. self.kin_flush_delay = SDS_CHECK_TIME
self.kin_flush_times = [] self.kin_flush_times = []
self.last_kin_flush_time = self.last_kin_move_time = 0. self.last_kin_flush_time = self.last_kin_move_time = 0.
# Setup iterative solver # Setup iterative solver
@ -512,7 +513,7 @@ class ToolHead:
self.kin_flush_times.pop(self.kin_flush_times.index(old_delay)) self.kin_flush_times.pop(self.kin_flush_times.index(old_delay))
if delay: if delay:
self.kin_flush_times.append(delay) self.kin_flush_times.append(delay)
new_delay = max(self.kin_flush_times + [0.]) new_delay = max(self.kin_flush_times + [SDS_CHECK_TIME])
self.kin_flush_delay = new_delay self.kin_flush_delay = new_delay
def register_lookahead_callback(self, callback): def register_lookahead_callback(self, callback):
last_move = self.move_queue.get_last() last_move = self.move_queue.get_last()