input_shaper: Factored out AxisInputShaper class

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>
This commit is contained in:
Dmitry Butyugin 2021-10-24 17:00:40 +02:00 committed by KevinOConnor
parent d5a7a7f00f
commit 54e21a7c26
3 changed files with 129 additions and 113 deletions

View File

@ -140,9 +140,8 @@ defs_kin_extruder = """
defs_kin_shaper = """ defs_kin_shaper = """
double input_shaper_get_step_generation_window(int n, double a[] double input_shaper_get_step_generation_window(int n, double a[]
, double t[]); , double t[]);
int input_shaper_set_shaper_params(struct stepper_kinematics *sk int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n_x, double a_x[], double t_x[] , int n, double a[], double t[]);
, int n_y, double a_y[], double t_y[]);
int input_shaper_set_sk(struct stepper_kinematics *sk int input_shaper_set_sk(struct stepper_kinematics *sk
, struct stepper_kinematics *orig_sk); , struct stepper_kinematics *orig_sk);
struct stepper_kinematics * input_shaper_alloc(void); struct stepper_kinematics * input_shaper_alloc(void);

View File

@ -38,12 +38,12 @@ shift_pulses(struct shaper_pulses *sp)
sp->pulses[i].t -= ts; sp->pulses[i].t -= ts;
} }
static void static int
init_shaper(int n, double a[], double t[], struct shaper_pulses *sp) init_shaper(int n, double a[], double t[], struct shaper_pulses *sp)
{ {
if (n < 0 || n > ARRAY_SIZE(sp->pulses)) { if (n < 0 || n > ARRAY_SIZE(sp->pulses)) {
sp->num_pulses = 0; sp->num_pulses = 0;
return; return -1;
} }
int i; int i;
double sum_a = 0.; double sum_a = 0.;
@ -57,6 +57,7 @@ init_shaper(int n, double a[], double t[], struct shaper_pulses *sp)
} }
sp->num_pulses = n; sp->num_pulses = n;
shift_pulses(sp); shift_pulses(sp);
return 0;
} }
@ -192,21 +193,20 @@ shaper_note_generation_time(struct input_shaper *is)
} }
int __visible int __visible
input_shaper_set_shaper_params(struct stepper_kinematics *sk input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n_x, double a_x[], double t_x[] , int n, double a[], double t[])
, int n_y, double a_y[], double t_y[])
{ {
if (axis != 'x' && axis != 'y')
return -1;
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (is->orig_sk->active_flags & AF_X) struct shaper_pulses *sp = axis == 'x' ? &is->sx : &is->sy;
init_shaper(n_x, a_x, t_x, &is->sx); int status = 0;
if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y))
status = init_shaper(n, a, t, sp);
else else
is->sx.num_pulses = 0; sp->num_pulses = 0;
if (is->orig_sk->active_flags & AF_Y)
init_shaper(n_y, a_y, t_y, &is->sy);
else
is->sy.num_pulses = 0;
shaper_note_generation_time(is); shaper_note_generation_time(is);
return 0; return status;
} }
double __visible double __visible

View File

@ -4,32 +4,99 @@
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com> # Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com>
# #
# 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 collections
import chelper import chelper
from . import shaper_defs from . import shaper_defs
class InputShaperParams:
def __init__(self, axis, config):
self.axis = axis
self.shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS}
self.shaper_type = config.get('shaper_type_' + axis, 'mzv').lower()
if self.shaper_type not in self.shapers:
raise config.error(
'Unsupported shaper type: %s' % (self.shaper_type,))
self.damping_ratio = config.getfloat('damping_ratio_' + axis,
shaper_defs.DEFAULT_DAMPING_RATIO,
minval=0., maxval=1.)
self.shaper_freq = config.getfloat('shaper_freq_' + axis, 0., minval=0.)
def update(self, gcmd):
axis = self.axis.upper()
self.damping_ratio = gcmd.get_float('DAMPING_RATIO_' + axis,
self.damping_ratio,
minval=0., maxval=1.)
self.shaper_freq = gcmd.get_float('SHAPER_FREQ_' + axis,
self.shaper_freq, minval=0.)
shaper_type = gcmd.get('SHAPER_TYPE', None)
if shaper_type is None:
shaper_type = gcmd.get('SHAPER_TYPE_' + axis, self.shaper_type)
if shaper_type.lower() not in self.shapers:
raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,))
self.shaper_type = shaper_type.lower()
def get_shaper(self):
if not self.shaper_freq:
A, T = shaper_defs.get_none_shaper()
else:
A, T = self.shapers[self.shaper_type](
self.shaper_freq, self.damping_ratio)
return len(A), A, T
def get_status(self):
return collections.OrderedDict([
('shaper_type', self.shaper_type),
('shaper_freq', '%.3f' % (self.shaper_freq,)),
('damping_ratio', '%.6f' % (self.damping_ratio,))])
class AxisInputShaper:
def __init__(self, axis, config):
self.axis = axis
self.params = InputShaperParams(axis, config)
self.n, self.A, self.T = self.params.get_shaper()
self.saved = None
def get_name(self):
return 'shaper_' + self.axis
def get_shaper(self):
return self.n, self.A, self.T
def update(self, gcmd):
self.params.update(gcmd)
old_n, old_A, old_T = self.n, self.A, self.T
self.n, self.A, self.T = self.params.get_shaper()
return (old_n, old_A, old_T) != (self.n, self.A, self.T)
def set_shaper_kinematics(self, sk):
ffi_main, ffi_lib = chelper.get_ffi()
success = ffi_lib.input_shaper_set_shaper_params(
sk, bytes(self.axis), self.n, self.A, self.T) == 0
if not success:
self.disable_shaping()
ffi_lib.input_shaper_set_shaper_params(
sk, bytes(self.axis), self.n, self.A, self.T)
return success
def get_step_generation_window(self):
ffi_main, ffi_lib = chelper.get_ffi()
return ffi_lib.input_shaper_get_step_generation_window(self.n,
self.A, self.T)
def disable_shaping(self):
if self.saved is None and self.n:
self.saved = (self.n, self.A, self.T)
A, T = shaper_defs.get_none_shaper()
self.n, self.A, self.T = len(A), A, T
def enable_shaping(self):
if self.saved is None:
# Input shaper was not disabled
return
self.n, self.A, self.T = self.saved
self.saved = None
def report(self, gcmd):
info = ' '.join(["%s_%s:%s" % (key, self.axis, value)
for (key, value) in self.params.get_status().items()])
gcmd.respond_info(info)
class InputShaper: class InputShaper:
def __init__(self, config): def __init__(self, config):
self.printer = config.get_printer() self.printer = config.get_printer()
self.printer.register_event_handler("klippy:connect", self.connect) self.printer.register_event_handler("klippy:connect", self.connect)
self.toolhead = None self.toolhead = None
self.shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS} self.shapers = [AxisInputShaper('x', config),
self.damping_ratio_x = config.getfloat( AxisInputShaper('y', config)]
'damping_ratio_x', shaper_defs.DEFAULT_DAMPING_RATIO,
minval=0., maxval=1.)
self.damping_ratio_y = config.getfloat(
'damping_ratio_y', shaper_defs.DEFAULT_DAMPING_RATIO,
minval=0., maxval=1.)
self.shaper_freq_x = config.getfloat('shaper_freq_x', 0., minval=0.)
self.shaper_freq_y = config.getfloat('shaper_freq_y', 0., minval=0.)
self.shaper_type_x = config.get('shaper_type_x', 'mzv').lower()
if self.shaper_type_x not in self.shapers:
raise config.error(
'Unsupported shaper type: %s' % (self.shaper_type_x,))
self.shaper_type_y = config.get('shaper_type_y', 'mzv').lower()
if self.shaper_type_y not in self.shapers:
raise config.error(
'Unsupported shaper type: %s' % (self.shaper_type_y,))
self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0.
self.stepper_kinematics = [] self.stepper_kinematics = []
self.orig_stepper_kinematics = [] self.orig_stepper_kinematics = []
# Register gcode commands # Register gcode commands
@ -37,6 +104,8 @@ class InputShaper:
gcode.register_command("SET_INPUT_SHAPER", gcode.register_command("SET_INPUT_SHAPER",
self.cmd_SET_INPUT_SHAPER, self.cmd_SET_INPUT_SHAPER,
desc=self.cmd_SET_INPUT_SHAPER_help) desc=self.cmd_SET_INPUT_SHAPER_help)
def get_shapers(self):
return self.shapers
def connect(self): def connect(self):
self.toolhead = self.printer.lookup_object("toolhead") self.toolhead = self.printer.lookup_object("toolhead")
kin = self.toolhead.get_kinematics() kin = self.toolhead.get_kinematics()
@ -54,92 +123,40 @@ class InputShaper:
self.orig_stepper_kinematics.append(orig_sk) self.orig_stepper_kinematics.append(orig_sk)
# Configure initial values # Configure initial values
self.old_delay = 0. self.old_delay = 0.
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, self._update_input_shaping(error=self.printer.config_error)
self.shaper_freq_x, self.shaper_freq_y, def _update_input_shaping(self, error=None):
self.damping_ratio_x, self.damping_ratio_y)
def _get_shaper(self, shaper_type, shaper_freq, damping_ratio):
if not shaper_freq:
return shaper_defs.get_none_shaper()
A, T = self.shapers[shaper_type](shaper_freq, damping_ratio)
return len(A), A, T
def _set_input_shaper(self, shaper_type_x, shaper_type_y
, shaper_freq_x, shaper_freq_y
, damping_ratio_x, damping_ratio_y):
if (shaper_type_x != self.shaper_type_x
or shaper_type_y != self.shaper_type_y):
self.toolhead.flush_step_generation() self.toolhead.flush_step_generation()
n_x, A_x, T_x = self._get_shaper( new_delay = max([s.get_step_generation_window() for s in self.shapers])
shaper_type_x, shaper_freq_x, damping_ratio_x)
n_y, A_y, T_y = self._get_shaper(
shaper_type_y, shaper_freq_y, damping_ratio_y)
ffi_main, ffi_lib = chelper.get_ffi()
new_delay = max(
ffi_lib.input_shaper_get_step_generation_window(n_x, A_x, T_x),
ffi_lib.input_shaper_get_step_generation_window(n_y, A_y, T_y))
self.toolhead.note_step_generation_scan_time(new_delay, self.toolhead.note_step_generation_scan_time(new_delay,
old_delay=self.old_delay) old_delay=self.old_delay)
self.old_delay = new_delay failed = []
self.shaper_type_x = shaper_type_x
self.shaper_type_y = shaper_type_y
self.shaper_freq_x = shaper_freq_x
self.shaper_freq_y = shaper_freq_y
self.damping_ratio_x = damping_ratio_x
self.damping_ratio_y = damping_ratio_y
for sk in self.stepper_kinematics: for sk in self.stepper_kinematics:
ffi_lib.input_shaper_set_shaper_params( for shaper in self.shapers:
sk, len(A_x), A_x, T_x, len(A_y), A_y, T_y) if shaper in failed:
continue
if not shaper.set_shaper_kinematics(sk):
failed.append(shaper)
if failed:
error = error or self.printer.command_error
raise error("Failed to configure shaper(s) %s with given parameters"
% (', '.join([s.get_name() for s in failed])))
def disable_shaping(self): def disable_shaping(self):
if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not ( for shaper in self.shapers:
self.shaper_freq_x or self.shaper_freq_y): shaper.disable_shaping()
# Input shaper is already disabled self._update_input_shaping()
return
self.saved_shaper_freq_x = self.shaper_freq_x
self.saved_shaper_freq_y = self.shaper_freq_y
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, 0., 0.,
self.damping_ratio_x, self.damping_ratio_y)
def enable_shaping(self): def enable_shaping(self):
saved = self.saved_shaper_freq_x or self.saved_shaper_freq_y for shaper in self.shapers:
if saved: shaper.enable_shaping()
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, self._update_input_shaping()
self.saved_shaper_freq_x,
self.saved_shaper_freq_y,
self.damping_ratio_x, self.damping_ratio_y)
self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0.
return saved
cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper" cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper"
def cmd_SET_INPUT_SHAPER(self, gcmd): def cmd_SET_INPUT_SHAPER(self, gcmd):
damping_ratio_x = gcmd.get_float( updated = False
'DAMPING_RATIO_X', self.damping_ratio_x, minval=0., maxval=1.) for shaper in self.shapers:
damping_ratio_y = gcmd.get_float( updated |= shaper.update(gcmd)
'DAMPING_RATIO_Y', self.damping_ratio_y, minval=0., maxval=1.) if updated:
shaper_freq_x = gcmd.get_float( self._update_input_shaping()
'SHAPER_FREQ_X', self.shaper_freq_x, minval=0.) for shaper in self.shapers:
shaper_freq_y = gcmd.get_float( shaper.report(gcmd)
'SHAPER_FREQ_Y', self.shaper_freq_y, minval=0.)
shaper_type = gcmd.get('SHAPER_TYPE', None)
if shaper_type is None:
shaper_type_x = gcmd.get(
'SHAPER_TYPE_X', self.shaper_type_x).lower()
shaper_type_y = gcmd.get(
'SHAPER_TYPE_Y', self.shaper_type_y).lower()
else:
shaper_type_x = shaper_type_y = shaper_type.lower()
if shaper_type_x not in self.shapers:
raise gcmd.error('Unsupported shaper type: %s' % (shaper_type_x,))
if shaper_type_y not in self.shapers:
raise gcmd.error('Unsupported shaper type: %s' % (shaper_type_y,))
self._set_input_shaper(shaper_type_x, shaper_type_y,
shaper_freq_x, shaper_freq_y,
damping_ratio_x, damping_ratio_y)
gcmd.respond_info("shaper_type_x:%s shaper_type_y:%s "
"shaper_freq_x:%.3f shaper_freq_y:%.3f "
"damping_ratio_x:%.6f damping_ratio_y:%.6f"
% (self.shaper_type_x, self.shaper_type_y,
self.shaper_freq_x, self.shaper_freq_y,
self.damping_ratio_x, self.damping_ratio_y))
def load_config(config): def load_config(config):
return InputShaper(config) return InputShaper(config)