aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--config/example-extras.cfg34
-rw-r--r--docs/G-Codes.md15
-rw-r--r--klippy/chelper/__init__.py26
-rw-r--r--klippy/chelper/kin_shaper.c421
-rw-r--r--klippy/extras/input_shaper.py132
5 files changed, 626 insertions, 2 deletions
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_x>]
+ [SHAPER_FREQ_Y=<shaper_freq_y>] [DAMPING_RATIO_X=<damping_ratio_x>]
+ [DAMPING_RATIO_Y=<damping_ratio_y>] [SHAPER_TYPE=<shaper>]
+ [SHAPER_TYPE_X=<shaper_type_x>] [SHAPER_TYPE_Y=<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 <kevin@koconnor.net>
+// Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <math.h> // sqrt, exp
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // 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 <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 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)
generated by cgit v1.2.3-70-g09d2 (git 2.50.1) at 2025-09-02 08:23:11 +0000