From 4bdc11a8b3843b73079a2c8dfc8d8c0928c29662 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Mon, 6 Jul 2020 02:54:38 +0200 Subject: [PATCH] input_shaper: Initial support of input shaping (#3032) Input shaping can help to reduce printer vibrations due to resonances and eliminate or reduce ghosting in prints. Signed-off-by: Dmitry Butyugin --- config/example-extras.cfg | 34 +++ docs/G-Codes.md | 15 ++ klippy/chelper/__init__.py | 26 ++- klippy/chelper/kin_shaper.c | 421 ++++++++++++++++++++++++++++++++++ klippy/extras/input_shaper.py | 132 +++++++++++ 5 files changed, 626 insertions(+), 2 deletions(-) create mode 100644 klippy/chelper/kin_shaper.c create mode 100644 klippy/extras/input_shaper.py diff --git a/config/example-extras.cfg b/config/example-extras.cfg index 8e7e0a5b..6ab4398b 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -522,6 +522,40 @@ # Directly sets the default prefix. If present, this value will override # the "default_type". +###################################################################### +# Resonance compensation +###################################################################### + +# Enables input shaping. +#[input_shaper] +#shaper_freq_x: 0 +# A frequency (in Hz) of the input shaper for X axis. This is usually a +# resonance frequency of X axis that the input shaper should suppress. +# For more complex shapers, like 2- and 3-hump EI input shapers, this +# parameter can be set from different considerations. +# The default value is 0, which disables input shaping for X axis. +#shaper_freq_y: 0 +# A frequency (in Hz) of the input shaper for Y axis. This is usually a +# resonance frequency of Y axis that the input shaper should suppress. +# For more complex shapers, like 2- and 3-hump EI input shapers, this +# parameter can be set from different considerations. +# The default value is 0, which disables input shaping for Y axis. +#shaper_type: mzv +# A type of the input shaper to use for both X and Y axes. Supported shapers +# are zv, mzv, zvd, ei, 2hump_ei, and 3hump_ei. +# The default is mzv input shaper. +#shaper_type_x: +#shaper_type_y: +# If shaper_type is not set, these two parameters can be used to configure +# different input shapers for X and Y axes. The same values are supported +# as for shaper_type parameter. +#damping_ratio_x: 0.1 +#damping_ratio_y: 0.1 +# Damping ratios of vibrations of X and Y axes used by input shapers to +# improve vibration suppression. Should not be changed without some proper +# measurements, e.g. with an accelerometer. +# Default value is 0.1 which is a good all-round value for most printers. + ###################################################################### # Config file helpers diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 3b939ec9..3c91ee63 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -596,3 +596,18 @@ been enabled: delay duration for the identified [delayed_gcode] and starts the timer for gcode execution. A value of 0 will cancel a pending delayed gcode from executing. + +## Resonance compensation + +The following command is enabled if an [input_shaper] config section has +been enabled: + - `SET_INPUT_SHAPER [SHAPER_FREQ_X=] + [SHAPER_FREQ_Y=] [DAMPING_RATIO_X=] + [DAMPING_RATIO_Y=] [SHAPER_TYPE=] + [SHAPER_TYPE_X=] [SHAPER_TYPE_Y=]`: Modify + input shaper parameters. Note that SHAPER_TYPE parameter resets input shaper + for both X and Y axes even if different shaper types have been configured + in [input_shaper] section. SHAPER_TYPE cannot be used together with either + of SHAPER_TYPE_X and SHAPER_TYPE_Y parameters. See + [example-extras.cfg](https://github.com/KevinOConnor/klipper/tree/master/config/example-extras.cfg) + for more details on each of these parameters. diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 1a4b2d74..d87c0b31 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -17,7 +17,7 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC" SOURCE_FILES = [ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.c', - 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c', + 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c', 'kin_shaper.c', ] DEST_LIB = "c_helper.so" OTHER_FILES = [ @@ -104,6 +104,27 @@ defs_kin_extruder = """ , double smooth_time); """ +defs_kin_shaper = """ + enum INPUT_SHAPER_TYPE { + INPUT_SHAPER_ZV = 0, + INPUT_SHAPER_ZVD = 1, + INPUT_SHAPER_MZV = 2, + INPUT_SHAPER_EI = 3, + INPUT_SHAPER_2HUMP_EI = 4, + INPUT_SHAPER_3HUMP_EI = 5, + }; + + double input_shaper_get_step_generation_window(int shaper_type + , double shaper_freq, double damping_ratio); + int input_shaper_set_shaper_params(struct stepper_kinematics *sk + , int shaper_type_x, int shaper_type_y + , double shaper_freq_x, double shaper_freq_y + , double damping_ratio_x, double damping_ratio_y); + int input_shaper_set_sk(struct stepper_kinematics *sk + , struct stepper_kinematics *orig_sk); + struct stepper_kinematics * input_shaper_alloc(void); +""" + defs_serialqueue = """ #define MESSAGE_MAX 64 struct pull_queue_message { @@ -147,7 +168,8 @@ defs_all = [ defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress, defs_itersolve, defs_trapq, defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar, - defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder + defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder, + defs_kin_shaper, ] # Return the list of file modification times diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c new file mode 100644 index 00000000..d79a9561 --- /dev/null +++ b/klippy/chelper/kin_shaper.c @@ -0,0 +1,421 @@ +// Kinematic input shapers to minimize motion vibrations in XY plane +// +// Copyright (C) 2019-2020 Kevin O'Connor +// Copyright (C) 2020 Dmitry Butyugin +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // sqrt, exp +#include // offsetof +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics +#include "trapq.h" // struct move + +/**************************************************************** + * Generic position calculation via shaper convolution + ****************************************************************/ + +static inline double +get_axis_position(struct move *m, int axis, double move_time) +{ + double axis_r = m->axes_r.axis[axis - 'x']; + double start_pos = m->start_pos.axis[axis - 'x']; + double move_dist = move_get_distance(m, move_time); + return start_pos + axis_r * move_dist; +} + +static inline double +get_axis_position_across_moves(struct move *m, int axis, double time) +{ + while (likely(time < 0.)) { + m = list_prev_entry(m, node); + time += m->move_t; + } + while (likely(time > m->move_t)) { + time -= m->move_t; + m = list_next_entry(m, node); + } + return get_axis_position(m, axis, time); +} + +struct shaper_pulse { + double t, a; +}; + +// Calculate the position from the convolution of the shaper with input signal +static inline double +calc_position(struct move *m, int axis, double move_time + , struct shaper_pulse *pulses, int n) +{ + double res = 0.; + for (int i = 0; i < n; ++i) + res += pulses[i].a * get_axis_position_across_moves( + m, axis, move_time + pulses[i].t); + return res; +} + +/**************************************************************** + * Shaper-specific initialization + ****************************************************************/ + +#define EI_SHAPER_VIB_TOL 0.05 + +enum INPUT_SHAPER_TYPE { + INPUT_SHAPER_ZV = 0, + INPUT_SHAPER_ZVD = 1, + INPUT_SHAPER_MZV = 2, + INPUT_SHAPER_EI = 3, + INPUT_SHAPER_2HUMP_EI = 4, + INPUT_SHAPER_3HUMP_EI = 5, +}; + +struct input_shaper { + struct stepper_kinematics sk; + struct stepper_kinematics *orig_sk; + struct move m; + struct shaper_pulse *x_pulses, *y_pulses; + int x_n, y_n; +}; + +typedef void (*is_init_shaper_callback)(double shaper_freq + , double damping_ratio + , struct shaper_pulse **pulses, int *n); + +static inline double +calc_ZV_K(double damping_ratio) +{ + if (likely(!damping_ratio)) + return 1.; + return exp(-damping_ratio * M_PI / sqrt(1. - damping_ratio*damping_ratio)); +} + +static inline double +calc_half_period(double shaper_freq, double damping_ratio) +{ + return .5 / (shaper_freq * sqrt(1. - damping_ratio*damping_ratio)); +} + +static void +init_shaper_zv(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 2; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = calc_ZV_K(damping_ratio); + double inv_D = 1. / (1. + K); + + (*pulses)[0].t = -half_period; + (*pulses)[1].t = 0.; + + (*pulses)[0].a = K * inv_D; + (*pulses)[1].a = inv_D; +} + +static void +init_shaper_zvd(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 3; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = calc_ZV_K(damping_ratio); + double K2 = K * K; + double inv_D = 1. / (K2 + 2. * K + 1.); + + (*pulses)[0].t = -2. * half_period; + (*pulses)[1].t = -half_period; + (*pulses)[2].t = 0.; + + (*pulses)[0].a = K2 * inv_D; + (*pulses)[1].a = 2. * K * inv_D; + (*pulses)[2].a = inv_D; +} + +static void +init_shaper_mzv(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 3; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = exp(-.75 * damping_ratio * M_PI + / sqrt(1. - damping_ratio*damping_ratio)); + + double a1 = 1. - 1. / sqrt(2.); + double a2 = (sqrt(2.) - 1.) * K; + double a3 = a1 * K * K; + double inv_D = 1. / (a1 + a2 + a3); + + (*pulses)[0].t = -1.5 * half_period; + (*pulses)[1].t = -.75 * half_period; + (*pulses)[2].t = 0.; + + (*pulses)[0].a = a3 * inv_D; + (*pulses)[1].a = a2 * inv_D; + (*pulses)[2].a = a1 * inv_D; +} + +static void +init_shaper_ei(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 3; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = calc_ZV_K(damping_ratio); + double a1 = .25 * (1. + EI_SHAPER_VIB_TOL); + double a2 = .5 * (1. - EI_SHAPER_VIB_TOL) * K; + double a3 = a1 * K * K; + double inv_D = 1. / (a1 + a2 + a3); + + (*pulses)[0].t = -2. * half_period; + (*pulses)[1].t = -half_period; + (*pulses)[2].t = 0.; + + (*pulses)[0].a = a3 * inv_D; + (*pulses)[1].a = a2 * inv_D; + (*pulses)[2].a = a1 * inv_D; +} + +static void +init_shaper_2hump_ei(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 4; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = calc_ZV_K(damping_ratio); + + double V2 = EI_SHAPER_VIB_TOL * EI_SHAPER_VIB_TOL; + double X = pow(V2 * (sqrt(1. - V2) + 1.), 1./3.); + double a1 = (3.*X*X + 2.*X + 3.*V2) / (16.*X); + double a2 = (.5 - a1) * K; + double a3 = a2 * K; + double a4 = a1 * K * K * K; + double inv_D = 1. / (a1 + a2 + a3 + a4); + + (*pulses)[0].t = -3. * half_period; + (*pulses)[1].t = -2. * half_period; + (*pulses)[2].t = -half_period; + (*pulses)[3].t = 0.; + + (*pulses)[0].a = a4 * inv_D; + (*pulses)[1].a = a3 * inv_D; + (*pulses)[2].a = a2 * inv_D; + (*pulses)[3].a = a1 * inv_D; +} + +static void +init_shaper_3hump_ei(double shaper_freq, double damping_ratio + , struct shaper_pulse **pulses, int *n) +{ + *n = 5; + *pulses = malloc(*n * sizeof(struct shaper_pulse)); + + double half_period = calc_half_period(shaper_freq, damping_ratio); + double K = calc_ZV_K(damping_ratio); + double K2 = K * K; + + double a1 = 0.0625 * (1. + 3. * EI_SHAPER_VIB_TOL + + 2. * sqrt(2. * (EI_SHAPER_VIB_TOL + 1.) * EI_SHAPER_VIB_TOL)); + double a2 = 0.25 * (1. - EI_SHAPER_VIB_TOL) * K; + double a3 = (0.5 * (1. + EI_SHAPER_VIB_TOL) - 2. * a1) * K2; + double a4 = a2 * K2; + double a5 = a1 * K2 * K2; + double inv_D = 1. / (a1 + a2 + a3 + a4 + a5); + + (*pulses)[0].t = -4. * half_period; + (*pulses)[1].t = -3. * half_period; + (*pulses)[2].t = -2. * half_period; + (*pulses)[3].t = -half_period; + (*pulses)[4].t = 0.; + + (*pulses)[0].a = a5 * inv_D; + (*pulses)[1].a = a4 * inv_D; + (*pulses)[2].a = a3 * inv_D; + (*pulses)[3].a = a2 * inv_D; + (*pulses)[4].a = a1 * inv_D; +} + +// Shift pulses around 'mid-point' t=0 so that the input shaper is an identity +// transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T) +static void +shift_pulses(int n, struct shaper_pulse *pulses) +{ + int i; + double ts = 0.; + for (i = 0; i < n; ++i) + ts += pulses[i].a * pulses[i].t; + for (i = 0; i < n; ++i) + pulses[i].t -= ts; +} + +/**************************************************************** + * Kinematics-related shaper code + ****************************************************************/ + +#define DUMMY_T 500.0 + +// Optimized calc_position when only x axis is needed +static double +shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + if (!is->x_n) + return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); + is->m.start_pos.x = calc_position(m, 'x', move_time, is->x_pulses, is->x_n); + return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); +} + +// Optimized calc_position when only y axis is needed +static double +shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + if (!is->y_n) + return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); + is->m.start_pos.y = calc_position(m, 'y', move_time, is->y_pulses, is->y_n); + return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); +} + +// General calc_position for both x and y axes +static double +shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + if (!is->x_n && !is->y_n) + return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); + is->m.start_pos = move_get_coord(m, move_time); + if (is->x_n) + is->m.start_pos.x = calc_position(m, 'x', move_time + , is->x_pulses, is->x_n); + if (is->y_n) + is->m.start_pos.y = calc_position(m, 'y', move_time + , is->y_pulses, is->y_n); + return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); +} + +static void +shaper_note_generation_time(struct input_shaper *is) +{ + double pre_active = 0., post_active = 0.; + if ((is->sk.active_flags & AF_X) && is->x_n) { + pre_active = is->x_pulses[is->x_n-1].t; + post_active = -is->x_pulses[0].t; + } + if ((is->sk.active_flags & AF_Y) && is->y_n) { + pre_active = is->y_pulses[is->y_n-1].t > pre_active + ? is->y_pulses[is->y_n-1].t : pre_active; + post_active = -is->y_pulses[0].t > post_active + ? -is->y_pulses[0].t : post_active; + } + is->sk.gen_steps_pre_active = pre_active; + is->sk.gen_steps_post_active = post_active; +} + +int __visible +input_shaper_set_sk(struct stepper_kinematics *sk + , struct stepper_kinematics *orig_sk) +{ + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + int af = orig_sk->active_flags & (AF_X | AF_Y); + if (af == (AF_X | AF_Y)) + is->sk.calc_position_cb = shaper_xy_calc_position; + else if (af & AF_X) + is->sk.calc_position_cb = shaper_x_calc_position; + else if (af & AF_Y) + is->sk.calc_position_cb = shaper_y_calc_position; + else + return -1; + is->sk.active_flags = orig_sk->active_flags; + is->orig_sk = orig_sk; + return 0; +} + +static is_init_shaper_callback init_shaper_callbacks[] = { + [INPUT_SHAPER_ZV] = &init_shaper_zv, + [INPUT_SHAPER_ZVD] = &init_shaper_zvd, + [INPUT_SHAPER_MZV] = &init_shaper_mzv, + [INPUT_SHAPER_EI] = &init_shaper_ei, + [INPUT_SHAPER_2HUMP_EI] = &init_shaper_2hump_ei, + [INPUT_SHAPER_3HUMP_EI] = &init_shaper_3hump_ei, +}; + +int __visible +input_shaper_set_shaper_params(struct stepper_kinematics *sk + , int shaper_type_x + , int shaper_type_y + , double shaper_freq_x + , double shaper_freq_y + , double damping_ratio_x + , double damping_ratio_y) +{ + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + + if (shaper_type_x >= ARRAY_SIZE(init_shaper_callbacks) || shaper_type_x < 0) + return -1; + if (shaper_type_y >= ARRAY_SIZE(init_shaper_callbacks) || shaper_type_y < 0) + return -1; + + int af = is->orig_sk->active_flags & (AF_X | AF_Y); + free(is->x_pulses); + if ((af & AF_X) && shaper_freq_x > 0.) { + init_shaper_callbacks[shaper_type_x]( + shaper_freq_x, damping_ratio_x, &is->x_pulses, &is->x_n); + shift_pulses(is->x_n, is->x_pulses); + } else { + is->x_pulses = NULL; + is->x_n = 0; + } + free(is->y_pulses); + if ((af & AF_Y) && shaper_freq_y > 0.) { + init_shaper_callbacks[shaper_type_y]( + shaper_freq_y, damping_ratio_y, &is->y_pulses, &is->y_n); + shift_pulses(is->y_n, is->y_pulses); + } else { + is->y_pulses = NULL; + is->y_n = 0; + } + shaper_note_generation_time(is); + return 0; +} + +double __visible +input_shaper_get_step_generation_window(int shaper_type, double shaper_freq + , double damping_ratio) +{ + if (shaper_freq <= 0.) + return 0.; + if (shaper_type >= ARRAY_SIZE(init_shaper_callbacks) || shaper_type < 0) + return 0.; + is_init_shaper_callback init_shaper_cb = init_shaper_callbacks[shaper_type]; + int n; + struct shaper_pulse *pulses; + init_shaper_cb(shaper_freq, damping_ratio, &pulses, &n); + shift_pulses(n, pulses); + double window = -pulses[0].t; + if (pulses[n-1].t > window) + window = pulses[n-1].t; + free(pulses); + return window; +} + +struct stepper_kinematics * __visible +input_shaper_alloc(void) +{ + struct input_shaper *is = malloc(sizeof(*is)); + memset(is, 0, sizeof(*is)); + is->m.move_t = 2. * DUMMY_T; + return &is->sk; +} diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py new file mode 100644 index 00000000..991d1db0 --- /dev/null +++ b/klippy/extras/input_shaper.py @@ -0,0 +1,132 @@ +# Kinematic input shaper to minimize motion vibrations in XY plane +# +# Copyright (C) 2019-2020 Kevin O'Connor +# Copyright (C) 2020 Dmitry Butyugin +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import chelper + +class InputShaper: + def __init__(self, config): + self.printer = config.get_printer() + self.printer.register_event_handler("klippy:connect", self.connect) + self.toolhead = None + self.damping_ratio_x = config.getfloat( + 'damping_ratio_x', 0.1, minval=0., maxval=1.) + self.damping_ratio_y = config.getfloat( + 'damping_ratio_y', 0.1, 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.) + ffi_main, ffi_lib = chelper.get_ffi() + self.shapers = {None: None + , 'zv': ffi_lib.INPUT_SHAPER_ZV + , 'zvd': ffi_lib.INPUT_SHAPER_ZVD + , 'mzv': ffi_lib.INPUT_SHAPER_MZV + , 'ei': ffi_lib.INPUT_SHAPER_EI + , '2hump_ei': ffi_lib.INPUT_SHAPER_2HUMP_EI + , '3hump_ei': ffi_lib.INPUT_SHAPER_3HUMP_EI} + shaper_type = config.getchoice('shaper_type', self.shapers, None) + if shaper_type is None: + self.shaper_type_x = config.getchoice( + 'shaper_type_x', self.shapers, 'mzv') + self.shaper_type_y = config.getchoice( + 'shaper_type_y', self.shapers, 'mzv') + else: + self.shaper_type_x = self.shaper_type_y = shaper_type + 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 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._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 _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() + ffi_main, ffi_lib = chelper.get_ffi() + new_delay = max( + ffi_lib.input_shaper_get_step_generation_window( + shaper_type_x, shaper_freq_x, damping_ratio_x), + ffi_lib.input_shaper_get_step_generation_window( + shaper_type_y, shaper_freq_y, damping_ratio_y)) + 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 + for sk in self.stepper_kinematics: + ffi_lib.input_shaper_set_shaper_params(sk + , shaper_type_x, shaper_type_y + , shaper_freq_x, shaper_freq_y + , damping_ratio_x, damping_ratio_y) + 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.) + + def parse_shaper(shaper_type_str): + shaper_type_str = shaper_type_str.lower() + if shaper_type_str not in self.shapers: + raise gcmd.error( + "Requested shaper type '%s' is not supported" % ( + shaper_type_str)) + return self.shapers[shaper_type_str] + + shaper_type = gcmd.get('SHAPER_TYPE', None, parser=parse_shaper) + if shaper_type is None: + shaper_type_x = gcmd.get('SHAPER_TYPE_X', self.shaper_type_x, + parser=parse_shaper) + shaper_type_y = gcmd.get('SHAPER_TYPE_Y', self.shaper_type_y, + parser=parse_shaper) + else: + shaper_type_x = shaper_type_y = shaper_type + + 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.shapers.keys()[ + self.shapers.values().index(shaper_type_x)] + , self.shapers.keys()[ + self.shapers.values().index(shaper_type_x)] + , shaper_freq_x, shaper_freq_y + , damping_ratio_x, damping_ratio_y)) + +def load_config(config): + return InputShaper(config)