klipper/klippy/extras/input_shaper.py

164 lines
7.0 KiB
Python
Raw Normal View History

# Kinematic input shaper to minimize motion vibrations in XY plane
#
# Copyright (C) 2019-2020 Kevin O'Connor <kevin@koconnor.net>
# 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}
shaper_type = config.get('shaper_type', 'mzv')
self.shaper_type = config.get('shaper_type_' + axis, shaper_type)
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, self.axis.encode(), self.n, self.A, self.T) == 0
if not success:
self.disable_shaping()
ffi_lib.input_shaper_set_shaper_params(
sk, self.axis.encode(), 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 = [AxisInputShaper('x', config),
AxisInputShaper('y', config)]
self.stepper_kinematics = []
self.orig_stepper_kinematics = []
# Register gcode commands
gcode = self.printer.lookup_object('gcode')
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()
# Lookup stepper kinematics
ffi_main, ffi_lib = chelper.get_ffi()
steppers = kin.get_steppers()
for s in steppers:
sk = ffi_main.gc(ffi_lib.input_shaper_alloc(), ffi_lib.free)
orig_sk = s.set_stepper_kinematics(sk)
res = ffi_lib.input_shaper_set_sk(sk, orig_sk)
if res < 0:
s.set_stepper_kinematics(orig_sk)
continue
self.stepper_kinematics.append(sk)
self.orig_stepper_kinematics.append(orig_sk)
# Configure initial values
self.old_delay = 0.
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)
failed = []
for sk in self.stepper_kinematics:
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):
for shaper in self.shapers:
shaper.disable_shaping()
self._update_input_shaping()
def enable_shaping(self):
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):
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)