diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 86560939..b5eb12dd 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -138,21 +138,11 @@ defs_kin_extruder = """ """ 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); + double input_shaper_get_step_generation_window(int n, double a[] + , double t[]); 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 n_x, double a_x[], double t_x[] + , int n_y, double a_y[], double t_y[]); int input_shaper_set_sk(struct stepper_kinematics *sk , struct stepper_kinematics *orig_sk); struct stepper_kinematics * input_shaper_alloc(void); diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c index f71650f1..efba013d 100644 --- a/klippy/chelper/kin_shaper.c +++ b/klippy/chelper/kin_shaper.c @@ -15,7 +15,7 @@ /**************************************************************** - * Shaper-specific initialization + * Shaper initialization ****************************************************************/ struct shaper_pulses { @@ -25,164 +25,6 @@ struct shaper_pulses { } 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 // transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T) static void @@ -196,38 +38,24 @@ shift_pulses(struct shaper_pulses *sp) 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 -init_shaper(int shaper_type, double shaper_freq, double damping_ratio - , struct shaper_pulses *sp) +init_shaper(int n, double a[], double t[], struct shaper_pulses *sp) { - if (shaper_type < 0 || shaper_type >= ARRAY_SIZE(init_shaper_callbacks) - || shaper_freq <= 0.) { + if (n < 0 || n > ARRAY_SIZE(sp->pulses)) { sp->num_pulses = 0; 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); } @@ -365,20 +193,16 @@ shaper_note_generation_time(struct input_shaper *is) 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) + , int n_x, double a_x[], double t_x[] + , int n_y, double a_y[], double t_y[]) { struct input_shaper *is = container_of(sk, struct input_shaper, sk); 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 is->sx.num_pulses = 0; 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 is->sy.num_pulses = 0; shaper_note_generation_time(is); @@ -386,11 +210,10 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk } double __visible -input_shaper_get_step_generation_window(int shaper_type, double shaper_freq - , double damping_ratio) +input_shaper_get_step_generation_window(int n, double a[], double t[]) { struct shaper_pulses sp; - init_shaper(shaper_type, shaper_freq, damping_ratio, &sp); + init_shaper(n, a, t, &sp); if (!sp.num_pulses) return 0.; double window = -sp.pulses[0].t; diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 69ac3cde..1d8fe0c7 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -5,31 +5,30 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import chelper +from . import shaper_defs class InputShaper: def __init__(self, config): self.printer = config.get_printer() self.printer.register_event_handler("klippy:connect", self.connect) self.toolhead = None + self.shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS} 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( - '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_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.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.shaper_type_x = config.get('shaper_type_x', 'mzv').lower() + if self.shaper_type_x not in self.shapers: + raise config.error( + 'Unsupported shaper type: %s' % (self.shaper_type_x,)) + self.shaper_type_y = config.get('shaper_type_y', 'mzv').lower() + if self.shaper_type_y not in self.shapers: + raise config.error( + 'Unsupported shaper type: %s' % (self.shaper_type_y,)) self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0. self.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.shaper_freq_x, self.shaper_freq_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 , 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() + 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() 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)) + ffi_lib.input_shaper_get_step_generation_window(n_x, A_x, T_x), + ffi_lib.input_shaper_get_step_generation_window(n_y, A_y, T_y)) self.toolhead.note_step_generation_scan_time(new_delay, old_delay=self.old_delay) self.old_delay = new_delay @@ -80,10 +86,8 @@ class InputShaper: 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) + ffi_lib.input_shaper_set_shaper_params( + sk, len(A_x), A_x, T_x, len(A_y), A_y, T_y) def disable_shaping(self): if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not ( self.shaper_freq_x or self.shaper_freq_y): @@ -113,35 +117,29 @@ class InputShaper: 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) + shaper_type = gcmd.get('SHAPER_TYPE', None) 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) + shaper_type_x = gcmd.get( + 'SHAPER_TYPE_X', self.shaper_type_x).lower() + shaper_type_y = gcmd.get( + 'SHAPER_TYPE_Y', self.shaper_type_y).lower() 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, shaper_freq_x, shaper_freq_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 " "shaper_freq_x:%.3f shaper_freq_y:%.3f " "damping_ratio_x:%.6f damping_ratio_y:%.6f" - % (id_to_name[shaper_type_x], - id_to_name[shaper_type_y], - shaper_freq_x, shaper_freq_y, - damping_ratio_x, damping_ratio_y)) + % (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 load_config(config): return InputShaper(config) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index cc0f0593..398ad78e 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -4,128 +4,16 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import collections, importlib, logging, math, multiprocessing +shaper_defs = importlib.import_module('.shaper_defs', 'extras') MIN_FREQ = 5. MAX_FREQ = 200. WINDOW_T_SEC = 0.5 MAX_SHAPER_FREQ = 150. -SHAPER_VIBRATION_REDUCTION=20. TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15] -SHAPER_DAMPING_RATIO = 0.1 -###################################################################### -# 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.), -] +AUTOTUNE_SHAPERS = ['zv', 'mzv', 'ei', '2hump_ei', '3hump_ei'] ###################################################################### # Frequency response calculation and shaper auto-tuning @@ -313,12 +201,32 @@ class ShaperCalibrate: # The input shaper can only reduce the amplitude of vibrations by # SHAPER_VIBRATION_REDUCTION times, so all vibrations below that # 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( - vals * psd - vibrations_threshold, 0).sum() - all_vibrations = self.numpy.maximum(psd - vibrations_threshold, 0).sum() + vals * psd - vibr_threshold, 0).sum() + all_vibrations = self.numpy.maximum(psd - vibr_threshold, 0).sum() 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): np = self.numpy @@ -333,8 +241,9 @@ class ShaperCalibrate: for test_freq in test_freqs[::-1]: shaper_vibrations = 0. shaper_vals = np.zeros(shape=freq_bins.shape) - shaper = shaper_cfg.init_func(test_freq, SHAPER_DAMPING_RATIO) - shaper_smoothing = get_shaper_smoothing(shaper) + shaper = shaper_cfg.init_func( + 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: return best_res # Exact damping ratio of the printer is unknown, pessimizing @@ -387,14 +296,16 @@ class ShaperCalibrate: # Just some empirically chosen value which produces good projections # for max_accel without much smoothing 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) return max_accel def find_best_shaper(self, calibration_data, max_smoothing, logger=None): best_shaper = None 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_cfg, calibration_data, max_smoothing)) if logger is not None: diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py new file mode 100644 index 00000000..611fed16 --- /dev/null +++ b/klippy/extras/shaper_defs.py @@ -0,0 +1,102 @@ +# Definitions of the supported input shapers +# +# Copyright (C) 2020-2021 Dmitry Butyugin +# +# 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.), +] diff --git a/scripts/calibrate_shaper.py b/scripts/calibrate_shaper.py index 5bbc9eea..3d4bceff 100755 --- a/scripts/calibrate_shaper.py +++ b/scripts/calibrate_shaper.py @@ -6,12 +6,12 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. from __future__ import print_function -import optparse, os, sys +import importlib, optparse, os, sys from textwrap import wrap import numpy as np, matplotlib sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', 'klippy', 'extras')) -from shaper_calibrate import CalibrationData, ShaperCalibrate + '..', 'klippy')) +shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras') MAX_TITLE_LENGTH=65 @@ -25,7 +25,7 @@ def parse_log(logname): return np.loadtxt(logname, comments='#', delimiter=',') # Parse power spectral density data data = np.loadtxt(logname, skiprows=1, comments='#', delimiter=',') - calibration_data = CalibrationData( + calibration_data = shaper_calibrate.CalibrationData( freq_bins=data[:,0], psd_sum=data[:,4], psd_x=data[:,1], psd_y=data[:,2], psd_z=data[:,3]) calibration_data.set_numpy(np) @@ -41,8 +41,8 @@ def parse_log(logname): # Find the best shaper parameters def calibrate_shaper(datas, csv_output, max_smoothing): - helper = ShaperCalibrate(printer=None) - if isinstance(datas[0], CalibrationData): + helper = shaper_calibrate.ShaperCalibrate(printer=None) + if isinstance(datas[0], shaper_calibrate.CalibrationData): calibration_data = datas[0] for data in datas[1:]: calibration_data.add_data(data) diff --git a/scripts/graph_accelerometer.py b/scripts/graph_accelerometer.py index 472530d3..990bd23b 100755 --- a/scripts/graph_accelerometer.py +++ b/scripts/graph_accelerometer.py @@ -5,12 +5,12 @@ # Copyright (C) 2020 Dmitry Butyugin # # 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 import numpy as np, matplotlib sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', 'klippy', 'extras')) -from shaper_calibrate import ShaperCalibrate + '..', 'klippy')) +shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras') MAX_TITLE_LENGTH=65 @@ -56,7 +56,7 @@ def plot_accel(data, logname): # Calculate estimated "power spectral density" def calc_freq_response(data, max_freq): - helper = ShaperCalibrate(printer=None) + helper = shaper_calibrate.ShaperCalibrate(printer=None) return helper.process_accelerometer_data(data) def calc_specgram(data, axis): @@ -155,7 +155,7 @@ def plot_specgram(data, logname, max_freq, axis): ###################################################################### def write_frequency_response(datas, output): - helper = ShaperCalibrate(printer=None) + helper = shaper_calibrate.ShaperCalibrate(printer=None) calibration_data = helper.process_accelerometer_data(datas[0]) for data in datas[1:]: calibration_data.add_data(helper.process_accelerometer_data(data))