mirror of https://github.com/Desuuuu/klipper.git
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 <dmbutyugin@google.com>
This commit is contained in:
parent
09a3d018a8
commit
4bdc11a8b3
|
@ -522,6 +522,40 @@
|
||||||
# Directly sets the default prefix. If present, this value will override
|
# Directly sets the default prefix. If present, this value will override
|
||||||
# the "default_type".
|
# 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
|
# Config file helpers
|
||||||
|
|
|
@ -596,3 +596,18 @@ been enabled:
|
||||||
delay duration for the identified [delayed_gcode] and starts the timer
|
delay duration for the identified [delayed_gcode] and starts the timer
|
||||||
for gcode execution. A value of 0 will cancel a pending delayed gcode
|
for gcode execution. A value of 0 will cancel a pending delayed gcode
|
||||||
from executing.
|
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.
|
||||||
|
|
|
@ -17,7 +17,7 @@ COMPILE_CMD = ("gcc -Wall -g -O2 -shared -fPIC"
|
||||||
SOURCE_FILES = [
|
SOURCE_FILES = [
|
||||||
'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
|
'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
|
||||||
'kin_cartesian.c', 'kin_corexy.c', 'kin_delta.c', 'kin_polar.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"
|
DEST_LIB = "c_helper.so"
|
||||||
OTHER_FILES = [
|
OTHER_FILES = [
|
||||||
|
@ -104,6 +104,27 @@ defs_kin_extruder = """
|
||||||
, double smooth_time);
|
, 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 = """
|
defs_serialqueue = """
|
||||||
#define MESSAGE_MAX 64
|
#define MESSAGE_MAX 64
|
||||||
struct pull_queue_message {
|
struct pull_queue_message {
|
||||||
|
@ -147,7 +168,8 @@ defs_all = [
|
||||||
defs_pyhelper, defs_serialqueue, defs_std,
|
defs_pyhelper, defs_serialqueue, defs_std,
|
||||||
defs_stepcompress, defs_itersolve, defs_trapq,
|
defs_stepcompress, defs_itersolve, defs_trapq,
|
||||||
defs_kin_cartesian, defs_kin_corexy, defs_kin_delta, defs_kin_polar,
|
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
|
# Return the list of file modification times
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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)
|
Loading…
Reference in New Issue