input_shaper: Define input shapers in a single place in Python code

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>
This commit is contained in:
Dmitry Butyugin 2021-10-22 20:46:20 +02:00 committed by KevinOConnor
parent 6c395fd016
commit d5a7a7f00f
7 changed files with 209 additions and 385 deletions

View File

@ -138,21 +138,11 @@ defs_kin_extruder = """
""" """
defs_kin_shaper = """ defs_kin_shaper = """
enum INPUT_SHAPER_TYPE { double input_shaper_get_step_generation_window(int n, double a[]
INPUT_SHAPER_ZV = 0, , double t[]);
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 input_shaper_set_shaper_params(struct stepper_kinematics *sk
, int shaper_type_x, int shaper_type_y , int n_x, double a_x[], double t_x[]
, double shaper_freq_x, double shaper_freq_y , int n_y, double a_y[], double t_y[]);
, double damping_ratio_x, double damping_ratio_y);
int input_shaper_set_sk(struct stepper_kinematics *sk int input_shaper_set_sk(struct stepper_kinematics *sk
, struct stepper_kinematics *orig_sk); , struct stepper_kinematics *orig_sk);
struct stepper_kinematics * input_shaper_alloc(void); struct stepper_kinematics * input_shaper_alloc(void);

View File

@ -15,7 +15,7 @@
/**************************************************************** /****************************************************************
* Shaper-specific initialization * Shaper initialization
****************************************************************/ ****************************************************************/
struct shaper_pulses { struct shaper_pulses {
@ -25,164 +25,6 @@ struct shaper_pulses {
} pulses[5]; } pulses[5];
}; };
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_pulses *sp)
{
sp->num_pulses = 2;
double half_period = calc_half_period(shaper_freq, damping_ratio);
double K = calc_ZV_K(damping_ratio);
double inv_D = 1. / (1. + K);
sp->pulses[0].t = -half_period;
sp->pulses[1].t = 0.;
sp->pulses[0].a = K * inv_D;
sp->pulses[1].a = inv_D;
}
static void
init_shaper_zvd(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp)
{
sp->num_pulses = 3;
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.);
sp->pulses[0].t = -2. * half_period;
sp->pulses[1].t = -half_period;
sp->pulses[2].t = 0.;
sp->pulses[0].a = K2 * inv_D;
sp->pulses[1].a = 2. * K * inv_D;
sp->pulses[2].a = inv_D;
}
static void
init_shaper_mzv(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp)
{
sp->num_pulses = 3;
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);
sp->pulses[0].t = -1.5 * half_period;
sp->pulses[1].t = -.75 * half_period;
sp->pulses[2].t = 0.;
sp->pulses[0].a = a3 * inv_D;
sp->pulses[1].a = a2 * inv_D;
sp->pulses[2].a = a1 * inv_D;
}
#define EI_SHAPER_VIB_TOL 0.05
static void
init_shaper_ei(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp)
{
sp->num_pulses = 3;
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);
sp->pulses[0].t = -2. * half_period;
sp->pulses[1].t = -half_period;
sp->pulses[2].t = 0.;
sp->pulses[0].a = a3 * inv_D;
sp->pulses[1].a = a2 * inv_D;
sp->pulses[2].a = a1 * inv_D;
}
static void
init_shaper_2hump_ei(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp)
{
sp->num_pulses = 4;
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);
sp->pulses[0].t = -3. * half_period;
sp->pulses[1].t = -2. * half_period;
sp->pulses[2].t = -half_period;
sp->pulses[3].t = 0.;
sp->pulses[0].a = a4 * inv_D;
sp->pulses[1].a = a3 * inv_D;
sp->pulses[2].a = a2 * inv_D;
sp->pulses[3].a = a1 * inv_D;
}
static void
init_shaper_3hump_ei(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp)
{
sp->num_pulses = 5;
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);
sp->pulses[0].t = -4. * half_period;
sp->pulses[1].t = -3. * half_period;
sp->pulses[2].t = -2. * half_period;
sp->pulses[3].t = -half_period;
sp->pulses[4].t = 0.;
sp->pulses[0].a = a5 * inv_D;
sp->pulses[1].a = a4 * inv_D;
sp->pulses[2].a = a3 * inv_D;
sp->pulses[3].a = a2 * inv_D;
sp->pulses[4].a = a1 * inv_D;
}
// Shift pulses around 'mid-point' t=0 so that the input shaper is an identity // 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) // transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T)
static void static void
@ -196,38 +38,24 @@ shift_pulses(struct shaper_pulses *sp)
sp->pulses[i].t -= ts; sp->pulses[i].t -= ts;
} }
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,
};
typedef void (*is_init_shaper_callback)(double shaper_freq
, double damping_ratio
, struct shaper_pulses *sp);
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,
};
static void static void
init_shaper(int shaper_type, double shaper_freq, double damping_ratio init_shaper(int n, double a[], double t[], struct shaper_pulses *sp)
, struct shaper_pulses *sp)
{ {
if (shaper_type < 0 || shaper_type >= ARRAY_SIZE(init_shaper_callbacks) if (n < 0 || n > ARRAY_SIZE(sp->pulses)) {
|| shaper_freq <= 0.) {
sp->num_pulses = 0; sp->num_pulses = 0;
return; return;
} }
init_shaper_callbacks[shaper_type](shaper_freq, damping_ratio, sp); int i;
double sum_a = 0.;
for (i = 0; i < n; ++i)
sum_a += a[i];
double inv_a = 1. / sum_a;
// Reverse pulses vs their traditional definition
for (i = 0; i < n; ++i) {
sp->pulses[n-i-1].a = a[i] * inv_a;
sp->pulses[n-i-1].t = -t[i];
}
sp->num_pulses = n;
shift_pulses(sp); shift_pulses(sp);
} }
@ -365,20 +193,16 @@ shaper_note_generation_time(struct input_shaper *is)
int __visible int __visible
input_shaper_set_shaper_params(struct stepper_kinematics *sk input_shaper_set_shaper_params(struct stepper_kinematics *sk
, int shaper_type_x , int n_x, double a_x[], double t_x[]
, int shaper_type_y , int n_y, double a_y[], double t_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); struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (is->orig_sk->active_flags & AF_X) if (is->orig_sk->active_flags & AF_X)
init_shaper(shaper_type_x, shaper_freq_x, damping_ratio_x, &is->sx); init_shaper(n_x, a_x, t_x, &is->sx);
else else
is->sx.num_pulses = 0; is->sx.num_pulses = 0;
if (is->orig_sk->active_flags & AF_Y) if (is->orig_sk->active_flags & AF_Y)
init_shaper(shaper_type_y, shaper_freq_y, damping_ratio_y, &is->sy); init_shaper(n_y, a_y, t_y, &is->sy);
else else
is->sy.num_pulses = 0; is->sy.num_pulses = 0;
shaper_note_generation_time(is); shaper_note_generation_time(is);
@ -386,11 +210,10 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk
} }
double __visible double __visible
input_shaper_get_step_generation_window(int shaper_type, double shaper_freq input_shaper_get_step_generation_window(int n, double a[], double t[])
, double damping_ratio)
{ {
struct shaper_pulses sp; struct shaper_pulses sp;
init_shaper(shaper_type, shaper_freq, damping_ratio, &sp); init_shaper(n, a, t, &sp);
if (!sp.num_pulses) if (!sp.num_pulses)
return 0.; return 0.;
double window = -sp.pulses[0].t; double window = -sp.pulses[0].t;

View File

@ -5,31 +5,30 @@
# #
# 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 chelper import chelper
from . import shaper_defs
class InputShaper: class InputShaper:
def __init__(self, config): def __init__(self, config):
self.printer = config.get_printer() self.printer = config.get_printer()
self.printer.register_event_handler("klippy:connect", self.connect) self.printer.register_event_handler("klippy:connect", self.connect)
self.toolhead = None self.toolhead = None
self.shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS}
self.damping_ratio_x = config.getfloat( self.damping_ratio_x = config.getfloat(
'damping_ratio_x', 0.1, minval=0., maxval=1.) 'damping_ratio_x', shaper_defs.DEFAULT_DAMPING_RATIO,
minval=0., maxval=1.)
self.damping_ratio_y = config.getfloat( self.damping_ratio_y = config.getfloat(
'damping_ratio_y', 0.1, minval=0., maxval=1.) 'damping_ratio_y', shaper_defs.DEFAULT_DAMPING_RATIO,
minval=0., maxval=1.)
self.shaper_freq_x = config.getfloat('shaper_freq_x', 0., minval=0.) self.shaper_freq_x = config.getfloat('shaper_freq_x', 0., minval=0.)
self.shaper_freq_y = config.getfloat('shaper_freq_y', 0., minval=0.) self.shaper_freq_y = config.getfloat('shaper_freq_y', 0., minval=0.)
ffi_main, ffi_lib = chelper.get_ffi() self.shaper_type_x = config.get('shaper_type_x', 'mzv').lower()
self.shapers = {None: None if self.shaper_type_x not in self.shapers:
, 'zv': ffi_lib.INPUT_SHAPER_ZV raise config.error(
, 'zvd': ffi_lib.INPUT_SHAPER_ZVD 'Unsupported shaper type: %s' % (self.shaper_type_x,))
, 'mzv': ffi_lib.INPUT_SHAPER_MZV self.shaper_type_y = config.get('shaper_type_y', 'mzv').lower()
, 'ei': ffi_lib.INPUT_SHAPER_EI if self.shaper_type_y not in self.shapers:
, '2hump_ei': ffi_lib.INPUT_SHAPER_2HUMP_EI raise config.error(
, '3hump_ei': ffi_lib.INPUT_SHAPER_3HUMP_EI} 'Unsupported shaper type: %s' % (self.shaper_type_y,))
shaper_type = config.get('shaper_type', 'mzv')
self.shaper_type_x = config.getchoice(
'shaper_type_x', self.shapers, shaper_type)
self.shaper_type_y = config.getchoice(
'shaper_type_y', self.shapers, shaper_type)
self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0. self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0.
self.stepper_kinematics = [] self.stepper_kinematics = []
self.orig_stepper_kinematics = [] self.orig_stepper_kinematics = []
@ -58,18 +57,25 @@ class InputShaper:
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, self._set_input_shaper(self.shaper_type_x, self.shaper_type_y,
self.shaper_freq_x, self.shaper_freq_y, self.shaper_freq_x, self.shaper_freq_y,
self.damping_ratio_x, self.damping_ratio_y) self.damping_ratio_x, self.damping_ratio_y)
def _get_shaper(self, shaper_type, shaper_freq, damping_ratio):
if not shaper_freq:
return shaper_defs.get_none_shaper()
A, T = self.shapers[shaper_type](shaper_freq, damping_ratio)
return len(A), A, T
def _set_input_shaper(self, shaper_type_x, shaper_type_y def _set_input_shaper(self, shaper_type_x, shaper_type_y
, shaper_freq_x, shaper_freq_y , shaper_freq_x, shaper_freq_y
, damping_ratio_x, damping_ratio_y): , damping_ratio_x, damping_ratio_y):
if (shaper_type_x != self.shaper_type_x if (shaper_type_x != self.shaper_type_x
or shaper_type_y != self.shaper_type_y): or shaper_type_y != self.shaper_type_y):
self.toolhead.flush_step_generation() self.toolhead.flush_step_generation()
n_x, A_x, T_x = self._get_shaper(
shaper_type_x, shaper_freq_x, damping_ratio_x)
n_y, A_y, T_y = self._get_shaper(
shaper_type_y, shaper_freq_y, damping_ratio_y)
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
new_delay = max( new_delay = max(
ffi_lib.input_shaper_get_step_generation_window( ffi_lib.input_shaper_get_step_generation_window(n_x, A_x, T_x),
shaper_type_x, shaper_freq_x, damping_ratio_x), ffi_lib.input_shaper_get_step_generation_window(n_y, A_y, T_y))
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, self.toolhead.note_step_generation_scan_time(new_delay,
old_delay=self.old_delay) old_delay=self.old_delay)
self.old_delay = new_delay self.old_delay = new_delay
@ -80,10 +86,8 @@ class InputShaper:
self.damping_ratio_x = damping_ratio_x self.damping_ratio_x = damping_ratio_x
self.damping_ratio_y = damping_ratio_y self.damping_ratio_y = damping_ratio_y
for sk in self.stepper_kinematics: for sk in self.stepper_kinematics:
ffi_lib.input_shaper_set_shaper_params(sk ffi_lib.input_shaper_set_shaper_params(
, shaper_type_x, shaper_type_y sk, len(A_x), A_x, T_x, len(A_y), A_y, T_y)
, shaper_freq_x, shaper_freq_y
, damping_ratio_x, damping_ratio_y)
def disable_shaping(self): def disable_shaping(self):
if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not ( if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not (
self.shaper_freq_x or self.shaper_freq_y): self.shaper_freq_x or self.shaper_freq_y):
@ -113,35 +117,29 @@ class InputShaper:
shaper_freq_y = gcmd.get_float( shaper_freq_y = gcmd.get_float(
'SHAPER_FREQ_Y', self.shaper_freq_y, minval=0.) 'SHAPER_FREQ_Y', self.shaper_freq_y, minval=0.)
def parse_shaper(shaper_type_str): shaper_type = gcmd.get('SHAPER_TYPE', None)
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: if shaper_type is None:
shaper_type_x = gcmd.get('SHAPER_TYPE_X', self.shaper_type_x, shaper_type_x = gcmd.get(
parser=parse_shaper) 'SHAPER_TYPE_X', self.shaper_type_x).lower()
shaper_type_y = gcmd.get('SHAPER_TYPE_Y', self.shaper_type_y, shaper_type_y = gcmd.get(
parser=parse_shaper) 'SHAPER_TYPE_Y', self.shaper_type_y).lower()
else: else:
shaper_type_x = shaper_type_y = shaper_type shaper_type_x = shaper_type_y = shaper_type.lower()
if shaper_type_x not in self.shapers:
raise gcmd.error('Unsupported shaper type: %s' % (shaper_type_x,))
if shaper_type_y not in self.shapers:
raise gcmd.error('Unsupported shaper type: %s' % (shaper_type_y,))
self._set_input_shaper(shaper_type_x, shaper_type_y, self._set_input_shaper(shaper_type_x, shaper_type_y,
shaper_freq_x, shaper_freq_y, shaper_freq_x, shaper_freq_y,
damping_ratio_x, damping_ratio_y) damping_ratio_x, damping_ratio_y)
id_to_name = {v: n for n, v in self.shapers.items()}
gcmd.respond_info("shaper_type_x:%s shaper_type_y:%s " gcmd.respond_info("shaper_type_x:%s shaper_type_y:%s "
"shaper_freq_x:%.3f shaper_freq_y:%.3f " "shaper_freq_x:%.3f shaper_freq_y:%.3f "
"damping_ratio_x:%.6f damping_ratio_y:%.6f" "damping_ratio_x:%.6f damping_ratio_y:%.6f"
% (id_to_name[shaper_type_x], % (self.shaper_type_x, self.shaper_type_y,
id_to_name[shaper_type_y], self.shaper_freq_x, self.shaper_freq_y,
shaper_freq_x, shaper_freq_y, self.damping_ratio_x, self.damping_ratio_y))
damping_ratio_x, damping_ratio_y))
def load_config(config): def load_config(config):
return InputShaper(config) return InputShaper(config)

View File

@ -4,128 +4,16 @@
# #
# 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, importlib, logging, math, multiprocessing import collections, importlib, logging, math, multiprocessing
shaper_defs = importlib.import_module('.shaper_defs', 'extras')
MIN_FREQ = 5. MIN_FREQ = 5.
MAX_FREQ = 200. MAX_FREQ = 200.
WINDOW_T_SEC = 0.5 WINDOW_T_SEC = 0.5
MAX_SHAPER_FREQ = 150. MAX_SHAPER_FREQ = 150.
SHAPER_VIBRATION_REDUCTION=20.
TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15] TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15]
SHAPER_DAMPING_RATIO = 0.1
###################################################################### AUTOTUNE_SHAPERS = ['zv', 'mzv', 'ei', '2hump_ei', '3hump_ei']
# Input shapers
######################################################################
InputShaperCfg = collections.namedtuple(
'InputShaperCfg', ('name', 'init_func', 'min_freq'))
def get_zv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., K]
T = [0., .5*t_d]
return (A, T)
def get_zvd_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., 2.*K, K**2]
T = [0., .5*t_d, t_d]
return (A, T)
def get_mzv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-.75 * damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = 1. - 1. / math.sqrt(2.)
a2 = (math.sqrt(2.) - 1.) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .375*t_d, .75*t_d]
return (A, T)
def get_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = .25 * (1. + v_tol)
a2 = .5 * (1. - v_tol) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .5*t_d, t_d]
return (A, T)
def get_2hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
V2 = v_tol**2
X = pow(V2 * (math.sqrt(1. - V2) + 1.), 1./3.)
a1 = (3.*X*X + 2.*X + 3.*V2) / (16.*X)
a2 = (.5 - a1) * K
a3 = a2 * K
a4 = a1 * K * K * K
A = [a1, a2, a3, a4]
T = [0., .5*t_d, t_d, 1.5*t_d]
return (A, T)
def get_3hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
K2 = K*K
a1 = 0.0625 * (1. + 3. * v_tol + 2. * math.sqrt(2. * (v_tol + 1.) * v_tol))
a2 = 0.25 * (1. - v_tol) * K
a3 = (0.5 * (1. + v_tol) - 2. * a1) * K2
a4 = a2 * K2
a5 = a1 * K2 * K2
A = [a1, a2, a3, a4, a5]
T = [0., .5*t_d, t_d, 1.5*t_d, 2.*t_d]
return (A, T)
def get_shaper_smoothing(shaper, accel=5000, scv=5.):
half_accel = accel * .5
A, T = shaper
inv_D = 1. / sum(A)
n = len(T)
# Calculate input shaper shift
ts = sum([A[i] * T[i] for i in range(n)]) * inv_D
# Calculate offset for 90 and 180 degrees turn
offset_90 = offset_180 = 0.
for i in range(n):
if T[i] >= ts:
# Calculate offset for one of the axes
offset_90 += A[i] * (scv + half_accel * (T[i]-ts)) * (T[i]-ts)
offset_180 += A[i] * half_accel * (T[i]-ts)**2
offset_90 *= inv_D * math.sqrt(2.)
offset_180 *= inv_D
return max(offset_90, offset_180)
# min_freq for each shaper is chosen to have projected max_accel ~= 1500
INPUT_SHAPERS = [
InputShaperCfg('zv', get_zv_shaper, min_freq=21.),
InputShaperCfg('mzv', get_mzv_shaper, min_freq=23.),
InputShaperCfg('ei', get_ei_shaper, min_freq=29.),
InputShaperCfg('2hump_ei', get_2hump_ei_shaper, min_freq=39.),
InputShaperCfg('3hump_ei', get_3hump_ei_shaper, min_freq=48.),
]
###################################################################### ######################################################################
# Frequency response calculation and shaper auto-tuning # Frequency response calculation and shaper auto-tuning
@ -313,12 +201,32 @@ class ShaperCalibrate:
# The input shaper can only reduce the amplitude of vibrations by # The input shaper can only reduce the amplitude of vibrations by
# SHAPER_VIBRATION_REDUCTION times, so all vibrations below that # SHAPER_VIBRATION_REDUCTION times, so all vibrations below that
# threshold can be igonred # threshold can be igonred
vibrations_threshold = psd.max() / SHAPER_VIBRATION_REDUCTION vibr_threshold = psd.max() / shaper_defs.SHAPER_VIBRATION_REDUCTION
remaining_vibrations = self.numpy.maximum( remaining_vibrations = self.numpy.maximum(
vals * psd - vibrations_threshold, 0).sum() vals * psd - vibr_threshold, 0).sum()
all_vibrations = self.numpy.maximum(psd - vibrations_threshold, 0).sum() all_vibrations = self.numpy.maximum(psd - vibr_threshold, 0).sum()
return (remaining_vibrations / all_vibrations, vals) return (remaining_vibrations / all_vibrations, vals)
def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.):
half_accel = accel * .5
A, T = shaper
inv_D = 1. / sum(A)
n = len(T)
# Calculate input shaper shift
ts = sum([A[i] * T[i] for i in range(n)]) * inv_D
# Calculate offset for 90 and 180 degrees turn
offset_90 = offset_180 = 0.
for i in range(n):
if T[i] >= ts:
# Calculate offset for one of the axes
offset_90 += A[i] * (scv + half_accel * (T[i]-ts)) * (T[i]-ts)
offset_180 += A[i] * half_accel * (T[i]-ts)**2
offset_90 *= inv_D * math.sqrt(2.)
offset_180 *= inv_D
return max(offset_90, offset_180)
def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing): def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing):
np = self.numpy np = self.numpy
@ -333,8 +241,9 @@ class ShaperCalibrate:
for test_freq in test_freqs[::-1]: for test_freq in test_freqs[::-1]:
shaper_vibrations = 0. shaper_vibrations = 0.
shaper_vals = np.zeros(shape=freq_bins.shape) shaper_vals = np.zeros(shape=freq_bins.shape)
shaper = shaper_cfg.init_func(test_freq, SHAPER_DAMPING_RATIO) shaper = shaper_cfg.init_func(
shaper_smoothing = get_shaper_smoothing(shaper) test_freq, shaper_defs.DEFAULT_DAMPING_RATIO)
shaper_smoothing = self._get_shaper_smoothing(shaper)
if max_smoothing and shaper_smoothing > max_smoothing and best_res: if max_smoothing and shaper_smoothing > max_smoothing and best_res:
return best_res return best_res
# Exact damping ratio of the printer is unknown, pessimizing # Exact damping ratio of the printer is unknown, pessimizing
@ -387,14 +296,16 @@ class ShaperCalibrate:
# Just some empirically chosen value which produces good projections # Just some empirically chosen value which produces good projections
# for max_accel without much smoothing # for max_accel without much smoothing
TARGET_SMOOTHING = 0.12 TARGET_SMOOTHING = 0.12
max_accel = self._bisect(lambda test_accel: get_shaper_smoothing( max_accel = self._bisect(lambda test_accel: self._get_shaper_smoothing(
shaper, test_accel) <= TARGET_SMOOTHING) shaper, test_accel) <= TARGET_SMOOTHING)
return max_accel return max_accel
def find_best_shaper(self, calibration_data, max_smoothing, logger=None): def find_best_shaper(self, calibration_data, max_smoothing, logger=None):
best_shaper = None best_shaper = None
all_shapers = [] all_shapers = []
for shaper_cfg in INPUT_SHAPERS: for shaper_cfg in shaper_defs.INPUT_SHAPERS:
if shaper_cfg.name not in AUTOTUNE_SHAPERS:
continue
shaper = self.background_process_exec(self.fit_shaper, ( shaper = self.background_process_exec(self.fit_shaper, (
shaper_cfg, calibration_data, max_smoothing)) shaper_cfg, calibration_data, max_smoothing))
if logger is not None: if logger is not None:

View File

@ -0,0 +1,102 @@
# Definitions of the supported input shapers
#
# Copyright (C) 2020-2021 Dmitry Butyugin <dmbutyugin@google.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import collections, math
SHAPER_VIBRATION_REDUCTION=20.
DEFAULT_DAMPING_RATIO = 0.1
InputShaperCfg = collections.namedtuple(
'InputShaperCfg', ('name', 'init_func', 'min_freq'))
def get_none_shaper():
return ([], [])
def get_zv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., K]
T = [0., .5*t_d]
return (A, T)
def get_zvd_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., 2.*K, K**2]
T = [0., .5*t_d, t_d]
return (A, T)
def get_mzv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-.75 * damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = 1. - 1. / math.sqrt(2.)
a2 = (math.sqrt(2.) - 1.) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .375*t_d, .75*t_d]
return (A, T)
def get_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = .25 * (1. + v_tol)
a2 = .5 * (1. - v_tol) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .5*t_d, t_d]
return (A, T)
def get_2hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
V2 = v_tol**2
X = pow(V2 * (math.sqrt(1. - V2) + 1.), 1./3.)
a1 = (3.*X*X + 2.*X + 3.*V2) / (16.*X)
a2 = (.5 - a1) * K
a3 = a2 * K
a4 = a1 * K * K * K
A = [a1, a2, a3, a4]
T = [0., .5*t_d, t_d, 1.5*t_d]
return (A, T)
def get_3hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 1. / SHAPER_VIBRATION_REDUCTION # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
K2 = K*K
a1 = 0.0625 * (1. + 3. * v_tol + 2. * math.sqrt(2. * (v_tol + 1.) * v_tol))
a2 = 0.25 * (1. - v_tol) * K
a3 = (0.5 * (1. + v_tol) - 2. * a1) * K2
a4 = a2 * K2
a5 = a1 * K2 * K2
A = [a1, a2, a3, a4, a5]
T = [0., .5*t_d, t_d, 1.5*t_d, 2.*t_d]
return (A, T)
# min_freq for each shaper is chosen to have projected max_accel ~= 1500
INPUT_SHAPERS = [
InputShaperCfg('zv', get_zv_shaper, min_freq=21.),
InputShaperCfg('mzv', get_mzv_shaper, min_freq=23.),
InputShaperCfg('zvd', get_zvd_shaper, min_freq=29.),
InputShaperCfg('ei', get_ei_shaper, min_freq=29.),
InputShaperCfg('2hump_ei', get_2hump_ei_shaper, min_freq=39.),
InputShaperCfg('3hump_ei', get_3hump_ei_shaper, min_freq=48.),
]

View File

@ -6,12 +6,12 @@
# #
# 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.
from __future__ import print_function from __future__ import print_function
import optparse, os, sys import importlib, optparse, os, sys
from textwrap import wrap from textwrap import wrap
import numpy as np, matplotlib import numpy as np, matplotlib
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'klippy', 'extras')) '..', 'klippy'))
from shaper_calibrate import CalibrationData, ShaperCalibrate shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras')
MAX_TITLE_LENGTH=65 MAX_TITLE_LENGTH=65
@ -25,7 +25,7 @@ def parse_log(logname):
return np.loadtxt(logname, comments='#', delimiter=',') return np.loadtxt(logname, comments='#', delimiter=',')
# Parse power spectral density data # Parse power spectral density data
data = np.loadtxt(logname, skiprows=1, comments='#', delimiter=',') data = np.loadtxt(logname, skiprows=1, comments='#', delimiter=',')
calibration_data = CalibrationData( calibration_data = shaper_calibrate.CalibrationData(
freq_bins=data[:,0], psd_sum=data[:,4], freq_bins=data[:,0], psd_sum=data[:,4],
psd_x=data[:,1], psd_y=data[:,2], psd_z=data[:,3]) psd_x=data[:,1], psd_y=data[:,2], psd_z=data[:,3])
calibration_data.set_numpy(np) calibration_data.set_numpy(np)
@ -41,8 +41,8 @@ def parse_log(logname):
# Find the best shaper parameters # Find the best shaper parameters
def calibrate_shaper(datas, csv_output, max_smoothing): def calibrate_shaper(datas, csv_output, max_smoothing):
helper = ShaperCalibrate(printer=None) helper = shaper_calibrate.ShaperCalibrate(printer=None)
if isinstance(datas[0], CalibrationData): if isinstance(datas[0], shaper_calibrate.CalibrationData):
calibration_data = datas[0] calibration_data = datas[0]
for data in datas[1:]: for data in datas[1:]:
calibration_data.add_data(data) calibration_data.add_data(data)

View File

@ -5,12 +5,12 @@
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com> # Copyright (C) 2020 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 optparse, os, sys import importlib, optparse, os, sys
from textwrap import wrap from textwrap import wrap
import numpy as np, matplotlib import numpy as np, matplotlib
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'klippy', 'extras')) '..', 'klippy'))
from shaper_calibrate import ShaperCalibrate shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras')
MAX_TITLE_LENGTH=65 MAX_TITLE_LENGTH=65
@ -56,7 +56,7 @@ def plot_accel(data, logname):
# Calculate estimated "power spectral density" # Calculate estimated "power spectral density"
def calc_freq_response(data, max_freq): def calc_freq_response(data, max_freq):
helper = ShaperCalibrate(printer=None) helper = shaper_calibrate.ShaperCalibrate(printer=None)
return helper.process_accelerometer_data(data) return helper.process_accelerometer_data(data)
def calc_specgram(data, axis): def calc_specgram(data, axis):
@ -155,7 +155,7 @@ def plot_specgram(data, logname, max_freq, axis):
###################################################################### ######################################################################
def write_frequency_response(datas, output): def write_frequency_response(datas, output):
helper = ShaperCalibrate(printer=None) helper = shaper_calibrate.ShaperCalibrate(printer=None)
calibration_data = helper.process_accelerometer_data(datas[0]) calibration_data = helper.process_accelerometer_data(datas[0])
for data in datas[1:]: for data in datas[1:]:
calibration_data.add_data(helper.process_accelerometer_data(data)) calibration_data.add_data(helper.process_accelerometer_data(data))