Druk-3D/klipper
1
0
Fork 0
mirror of https://github.com/Klipper3d/klipper.git synced 2025-10-26 15:56:10 +01:00
Code Issues Packages Projects Releases Wiki Activity

input_shaper: Z-axis input shaper

Browse Source 
Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>
This commit is contained in:
Dmitry Butyugin
2025-09-19 21:46:56 +02:00
committed by KevinOConnor
parent c49dbb5a87
commit caf7accf2d
3 changed files with 73 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
klippy/chelper/kin_shaper.c
View File

@@ -1,7 +1,7 @@
// Kinematic input shapers to minimize motion vibrations in XY plane // Kinematic input shapers to minimize motion vibrations in XY plane
// //
// Copyright (C) 2019-2020 Kevin O'Connor <kevin@koconnor.net> // Copyright (C) 2019-2020 Kevin O'Connor <kevin@koconnor.net>
// Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com> // Copyright (C) 2020-2025 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.
@@ -18,6 +18,8 @@
* Shaper initialization * Shaper initialization
****************************************************************/ ****************************************************************/
static const int KIN_FLAGS[3] = { AF_X, AF_Y, AF_Z };
struct shaper_pulses { struct shaper_pulses {
int num_pulses; int num_pulses;
struct { struct {
@@ -113,7 +115,7 @@ struct input_shaper {
struct stepper_kinematics sk; struct stepper_kinematics sk;
struct stepper_kinematics *orig_sk; struct stepper_kinematics *orig_sk;
struct move m; struct move m;
struct shaper_pulses sx, sy; struct shaper_pulses sp[3];
}; };
// Optimized calc_position when only x axis is needed // Optimized calc_position when only x axis is needed
@@ -122,9 +124,10 @@ shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->sx.num_pulses) struct shaper_pulses *sx = &is->sp[0];
if (!sx->num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); 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->sx); is->m.start_pos.x = calc_position(m, 'x', move_time, sx);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
@@ -134,25 +137,41 @@ shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->sy.num_pulses) struct shaper_pulses *sy = &is->sp[1];
if (!sy->num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); 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->sy); is->m.start_pos.y = calc_position(m, 'y', move_time, sy);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
// General calc_position for both x and y axes // Optimized calc_position when only z axis is needed
static double static double
shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m shaper_z_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time) , double move_time)
{ {
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->sx.num_pulses && !is->sy.num_pulses) struct shaper_pulses *sz = &is->sp[2];
if (!sz->num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos.z = calc_position(m, 'z', move_time, sz);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
}
// General calc_position for all x, y, and z axes
static double
shaper_xyz_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->sp[0].num_pulses && !is->sp[1].num_pulses && !is->sp[2].num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos = move_get_coord(m, move_time); is->m.start_pos = move_get_coord(m, move_time);
if (is->sx.num_pulses) if (is->sp[0].num_pulses)
is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx); is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sp[0]);
if (is->sy.num_pulses) if (is->sp[1].num_pulses)
is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy); is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sp[1]);
if (is->sp[2].num_pulses)
is->m.start_pos.z = calc_position(m, 'z', move_time, &is->sp[2]);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
} }
@@ -170,15 +189,24 @@ static void
shaper_note_generation_time(struct input_shaper *is) shaper_note_generation_time(struct input_shaper *is)
{ {
double pre_active = 0., post_active = 0.; double pre_active = 0., post_active = 0.;
if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) { struct shaper_pulses *sx = &is->sp[0];
pre_active = is->sx.pulses[is->sx.num_pulses-1].t; if ((is->sk.active_flags & AF_X) && sx->num_pulses) {
post_active = -is->sx.pulses[0].t; pre_active = sx->pulses[sx->num_pulses-1].t;
post_active = -sx->pulses[0].t;
} }
if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) { struct shaper_pulses *sy = &is->sp[1];
pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active if ((is->sk.active_flags & AF_Y) && sy->num_pulses) {
? is->sy.pulses[is->sy.num_pulses-1].t : pre_active; pre_active = sy->pulses[sy->num_pulses-1].t > pre_active
post_active = -is->sy.pulses[0].t > post_active ? sy->pulses[sy->num_pulses-1].t : pre_active;
? -is->sy.pulses[0].t : post_active; post_active = -sy->pulses[0].t > post_active
? -sy->pulses[0].t : post_active;
}
struct shaper_pulses *sz = &is->sp[2];
if ((is->sk.active_flags & AF_Z) && sz->num_pulses) {
pre_active = sz->pulses[sz->num_pulses-1].t > pre_active
? sz->pulses[sz->num_pulses-1].t : pre_active;
post_active = -sz->pulses[0].t > post_active
? -sz->pulses[0].t : post_active;
} }
is->sk.gen_steps_pre_active = pre_active; is->sk.gen_steps_pre_active = pre_active;
is->sk.gen_steps_post_active = post_active; is->sk.gen_steps_post_active = post_active;
@@ -188,12 +216,15 @@ void __visible
input_shaper_update_sk(struct stepper_kinematics *sk) input_shaper_update_sk(struct stepper_kinematics *sk)
{ {
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 | AF_Y)) == (AF_X | AF_Y)) int kin_flags = is->orig_sk->active_flags & (AF_X | AF_Y | AF_Z);
is->sk.calc_position_cb = shaper_xy_calc_position; if ((kin_flags & AF_X) == AF_X)
else if (is->orig_sk->active_flags & AF_X)
is->sk.calc_position_cb = shaper_x_calc_position; is->sk.calc_position_cb = shaper_x_calc_position;
else if (is->orig_sk->active_flags & AF_Y) else if ((kin_flags & AF_Y) == AF_Y)
is->sk.calc_position_cb = shaper_y_calc_position; is->sk.calc_position_cb = shaper_y_calc_position;
else if ((kin_flags & AF_Z) == AF_Z)
is->sk.calc_position_cb = shaper_z_calc_position;
else
is->sk.calc_position_cb = shaper_xyz_calc_position;
is->sk.active_flags = is->orig_sk->active_flags; is->sk.active_flags = is->orig_sk->active_flags;
shaper_note_generation_time(is); shaper_note_generation_time(is);
} }
@@ -207,8 +238,10 @@ input_shaper_set_sk(struct stepper_kinematics *sk
is->sk.calc_position_cb = shaper_x_calc_position; is->sk.calc_position_cb = shaper_x_calc_position;
else if (orig_sk->active_flags == AF_Y) else if (orig_sk->active_flags == AF_Y)
is->sk.calc_position_cb = shaper_y_calc_position; is->sk.calc_position_cb = shaper_y_calc_position;
else if (orig_sk->active_flags & (AF_X | AF_Y)) else if (orig_sk->active_flags == AF_Z)
is->sk.calc_position_cb = shaper_xy_calc_position; is->sk.calc_position_cb = shaper_z_calc_position;
else if (orig_sk->active_flags & (AF_X | AF_Y | AF_Z))
is->sk.calc_position_cb = shaper_xyz_calc_position;
else else
return -1; return -1;
is->sk.active_flags = orig_sk->active_flags; is->sk.active_flags = orig_sk->active_flags;
@@ -226,13 +259,14 @@ int __visible
input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n, double a[], double t[]) , int n, double a[], double t[])
{ {
if (axis != 'x' && axis != 'y') int axis_ind = axis-'x';
if (axis_ind < 0 || axis_ind >= ARRAY_SIZE(KIN_FLAGS))
return -1; return -1;
struct input_shaper *is = container_of(sk, struct input_shaper, sk); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
struct shaper_pulses *sp = axis == 'x' ? &is->sx : &is->sy; struct shaper_pulses *sp = &is->sp[axis_ind];
int status = 0; int status = 0;
// Ignore input shaper update if the axis is not active // Ignore input shaper update if the axis is not active
if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y)) { if (is->orig_sk->active_flags & KIN_FLAGS[axis_ind]) {
status = init_shaper(n, a, t, sp); status = init_shaper(n, a, t, sp);
shaper_note_generation_time(is); shaper_note_generation_time(is);
} }

10
klippy/extras/input_shaper.py
View File

@@ -1,7 +1,7 @@
# Kinematic input shaper to minimize motion vibrations in XY plane # Kinematic input shaper to minimize motion vibrations in XY plane
# #
# Copyright (C) 2019-2020 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2019-2020 Kevin O'Connor <kevin@koconnor.net>
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com> # Copyright (C) 2020-2025 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 collections
@@ -100,7 +100,8 @@ class InputShaper:
self._update_kinematics) self._update_kinematics)
self.toolhead = None self.toolhead = None
self.shapers = [AxisInputShaper('x', config), self.shapers = [AxisInputShaper('x', config),
AxisInputShaper('y', config)] AxisInputShaper('y', config),
AxisInputShaper('z', config)]
self.input_shaper_stepper_kinematics = [] self.input_shaper_stepper_kinematics = []
self.orig_stepper_kinematics = [] self.orig_stepper_kinematics = []
# Register gcode commands # Register gcode commands
@@ -191,8 +192,9 @@ class InputShaper:
for shaper in self.shapers: for shaper in self.shapers:
shaper.update(gcmd) shaper.update(gcmd)
self._update_input_shaping() self._update_input_shaping()
for shaper in self.shapers: for ind, shaper in enumerate(self.shapers):
shaper.report(gcmd) if ind < 2 or shaper.is_enabled():
shaper.report(gcmd)
def load_config(config): def load_config(config):
return InputShaper(config) return InputShaper(config)

3
test/klippy/input_shaper.cfg
View File

@@ -71,8 +71,9 @@ max_z_accel: 100
shaper_type_x: mzv shaper_type_x: mzv
shaper_freq_x: 33.2 shaper_freq_x: 33.2
shaper_type_y: ei shaper_type_y: ei
shaper_freq_x: 39.3 shaper_freq_y: 39.3
damping_ratio_y: 0.4 damping_ratio_y: 0.4
shaper_freq_z: 42
[adxl345] [adxl345]
cs_pin: PK7 cs_pin: PK7