diff options
Diffstat (limited to 'klippy/extras/input_shaper.py')
| -rw-r--r-- | klippy/extras/input_shaper.py | 211 |
1 files changed, 114 insertions, 97 deletions
diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 1d8fe0c7..c4963c8d 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -4,32 +4,99 @@ # Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com> # # This file may be distributed under the terms of the GNU GPLv3 license. +import collections import chelper 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: def __init__(self, config): self.printer = config.get_printer() self.printer.register_event_handler("klippy:connect", self.connect) self.toolhead = None - self.shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS} - self.damping_ratio_x = config.getfloat( - '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.shapers = [AxisInputShaper('x', config), + AxisInputShaper('y', config)] self.stepper_kinematics = [] self.orig_stepper_kinematics = [] # Register gcode commands @@ -37,6 +104,8 @@ class InputShaper: gcode.register_command("SET_INPUT_SHAPER", self.cmd_SET_INPUT_SHAPER, desc=self.cmd_SET_INPUT_SHAPER_help) + def get_shapers(self): + return self.shapers def connect(self): self.toolhead = self.printer.lookup_object("toolhead") kin = self.toolhead.get_kinematics() @@ -54,92 +123,40 @@ class InputShaper: self.orig_stepper_kinematics.append(orig_sk) # Configure initial values self.old_delay = 0. - self._set_input_shaper(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 _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() - n_x, A_x, T_x = self._get_shaper( - 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._update_input_shaping(error=self.printer.config_error) + def _update_input_shaping(self, error=None): + self.toolhead.flush_step_generation() + new_delay = max([s.get_step_generation_window() for s in self.shapers]) self.toolhead.note_step_generation_scan_time(new_delay, old_delay=self.old_delay) - self.old_delay = new_delay - 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 + failed = [] for sk in self.stepper_kinematics: - ffi_lib.input_shaper_set_shaper_params( - sk, len(A_x), A_x, T_x, len(A_y), A_y, T_y) + for shaper in self.shapers: + 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): - if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not ( - self.shaper_freq_x or self.shaper_freq_y): - # Input shaper is already disabled - 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) + for shaper in self.shapers: + shaper.disable_shaping() + self._update_input_shaping() def enable_shaping(self): - saved = self.saved_shaper_freq_x or self.saved_shaper_freq_y - if saved: - self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, - 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 + for shaper in self.shapers: + shaper.enable_shaping() + self._update_input_shaping() cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper" def cmd_SET_INPUT_SHAPER(self, gcmd): - damping_ratio_x = gcmd.get_float( - 'DAMPING_RATIO_X', self.damping_ratio_x, minval=0., maxval=1.) - damping_ratio_y = gcmd.get_float( - 'DAMPING_RATIO_Y', self.damping_ratio_y, minval=0., maxval=1.) - shaper_freq_x = gcmd.get_float( - 'SHAPER_FREQ_X', self.shaper_freq_x, minval=0.) - shaper_freq_y = gcmd.get_float( - '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)) + updated = False + for shaper in self.shapers: + updated |= shaper.update(gcmd) + if updated: + self._update_input_shaping() + for shaper in self.shapers: + shaper.report(gcmd) def load_config(config): return InputShaper(config) |