3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kin_shaper: Group shaper setup code together 

No code changes - just code movement.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2020-08-30 11:47:06 -04:00
parent 0e21188cb2
commit d28e148ddc
1 changed files with 93 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
klippy/chelper/kin_shaper.c
View File

@ -13,33 +13,11 @@
#include "itersolve.h" // struct stepper_kinematics #include "itersolve.h" // struct stepper_kinematics
#include "trapq.h" // struct move #include "trapq.h" // struct move
/**************************************************************** /****************************************************************
* Generic position calculation via shaper convolution * Shaper-specific initialization
****************************************************************/ ****************************************************************/
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_pulses { struct shaper_pulses {
int num_pulses; int num_pulses;
struct { struct {
@ -47,46 +25,6 @@ struct shaper_pulses {
} pulses[5]; } pulses[5];
}; };
// 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_pulses *sp)
{
double res = 0.;
int num_pulses = sp->num_pulses, i;
for (i = 0; i < num_pulses; ++i) {
double t = sp->pulses[i].t, a = sp->pulses[i].a;
res += a * get_axis_position_across_moves(m, axis, move_time + 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_pulses sx, sy;
};
typedef void (*is_init_shaper_callback)(double shaper_freq
, double damping_ratio
, struct shaper_pulses *sp);
static inline double static inline double
calc_ZV_K(double damping_ratio) calc_ZV_K(double damping_ratio)
{ {
@ -162,6 +100,8 @@ init_shaper_mzv(double shaper_freq, double damping_ratio
sp->pulses[2].a = a1 * inv_D; sp->pulses[2].a = a1 * inv_D;
} }
#define EI_SHAPER_VIB_TOL 0.05
static void static void
init_shaper_ei(double shaper_freq, double damping_ratio init_shaper_ei(double shaper_freq, double damping_ratio
, struct shaper_pulses *sp) , struct shaper_pulses *sp)
@ -256,12 +196,84 @@ 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,
};
/****************************************************************
* 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);
}
// 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_pulses *sp)
{
double res = 0.;
int num_pulses = sp->num_pulses, i;
for (i = 0; i < num_pulses; ++i) {
double t = sp->pulses[i].t, a = sp->pulses[i].a;
res += a * get_axis_position_across_moves(m, axis, move_time + t);
}
return res;
}
/**************************************************************** /****************************************************************
* Kinematics-related shaper code * Kinematics-related shaper code
****************************************************************/ ****************************************************************/
#define DUMMY_T 500.0 #define DUMMY_T 500.0
struct input_shaper {
struct stepper_kinematics sk;
struct stepper_kinematics *orig_sk;
struct move m;
struct shaper_pulses sx, sy;
};
// Optimized calc_position when only x axis is needed // Optimized calc_position when only x axis is needed
static double static double
shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m
@ -302,24 +314,6 @@ shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m
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);
} }
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->sx.num_pulses) {
pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
post_active = -is->sx.pulses[0].t;
}
if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
post_active = -is->sy.pulses[0].t > post_active
? -is->sy.pulses[0].t : post_active;
}
is->sk.gen_steps_pre_active = pre_active;
is->sk.gen_steps_post_active = post_active;
}
int __visible int __visible
input_shaper_set_sk(struct stepper_kinematics *sk input_shaper_set_sk(struct stepper_kinematics *sk
, struct stepper_kinematics *orig_sk) , struct stepper_kinematics *orig_sk)
@ -339,14 +333,23 @@ input_shaper_set_sk(struct stepper_kinematics *sk
return 0; return 0;
} }
static is_init_shaper_callback init_shaper_callbacks[] = { static void
[INPUT_SHAPER_ZV] = &init_shaper_zv, shaper_note_generation_time(struct input_shaper *is)
[INPUT_SHAPER_ZVD] = &init_shaper_zvd, {
[INPUT_SHAPER_MZV] = &init_shaper_mzv, double pre_active = 0., post_active = 0.;
[INPUT_SHAPER_EI] = &init_shaper_ei, if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) {
[INPUT_SHAPER_2HUMP_EI] = &init_shaper_2hump_ei, pre_active = is->sx.pulses[is->sx.num_pulses-1].t;
[INPUT_SHAPER_3HUMP_EI] = &init_shaper_3hump_ei, post_active = -is->sx.pulses[0].t;
}; }
if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) {
pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active
? is->sy.pulses[is->sy.num_pulses-1].t : pre_active;
post_active = -is->sy.pulses[0].t > post_active
? -is->sy.pulses[0].t : post_active;
}
is->sk.gen_steps_pre_active = pre_active;
is->sk.gen_steps_post_active = post_active;
}
int __visible int __visible
input_shaper_set_shaper_params(struct stepper_kinematics *sk input_shaper_set_shaper_params(struct stepper_kinematics *sk