Skip to content

Commit

Permalink
extruder: Sync extruder motion with input shaping
Browse files Browse the repository at this point in the history 
Signed-off-by: Dmitry Butyugin <[email protected]>
  • Loading branch information
@dmbutyugin @rogerlz
dmbutyugin authored and rogerlz committed Nov 22, 2024
1 parent f0248a3 commit a893f98
Show file tree
Hide file tree
Showing 6 changed files with 315 additions and 96 deletions.
7 changes: 5 additions & 2 deletions klippy/chelper/__init__.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@
"trapq.h",
"pollreactor.h",
"msgblock.h",
"kin_shaper.h",
]

defs_stepcompress = """
Expand Down Expand Up @@ -176,11 +177,13 @@
, double pa_velocity, struct pressure_advance_params *pa_params);
void extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk
, double (*func)(double, double, struct pressure_advance_params *));
int extruder_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n, double a[], double t[]);
double extruder_get_step_gen_window(struct stepper_kinematics *sk);
"""

defs_kin_shaper = """
double input_shaper_get_step_generation_window(
struct stepper_kinematics *sk);
double input_shaper_get_step_gen_window(struct stepper_kinematics *sk);
int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n, double a[], double t[]);
int input_shaper_set_sk(struct stepper_kinematics *sk
Expand Down
75 changes: 71 additions & 4 deletions klippy/chelper/kin_extruder.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
#include "kin_shaper.h" // struct shaper_pulses
#include "pyhelper.h" // errorf
#include "trapq.h" // move_get_distance

Expand Down Expand Up @@ -89,6 +90,14 @@ static void
pa_range_integrate(struct move *m, int axis, double move_time, double hst
, double *pos_integral, double *pa_velocity_integral)
{
while (unlikely(move_time < 0.)) {
m = list_prev_entry(m, node);
move_time += m->move_t;
}
while (unlikely(move_time > m->move_t)) {
move_time -= m->move_t;
m = list_next_entry(m, node);
}
// Calculate integral for the current move
*pos_integral = *pa_velocity_integral = 0.;
double m_pos_int, m_pa_vel_int;
Expand Down Expand Up @@ -123,6 +132,24 @@ pa_range_integrate(struct move *m, int axis, double move_time, double hst
*pos_integral -= m_pos_int;
*pa_velocity_integral -= m_pa_vel_int;
}
*pos_integral += start_base * hst * hst;
}

static void
shaper_pa_range_integrate(struct move *m, int axis, double move_time
, double hst, struct shaper_pulses *sp
, double *pos_integral, double *pa_velocity_integral)
{
*pos_integral = *pa_velocity_integral = 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;
double p_pos_int, p_pa_vel_int;
pa_range_integrate(m, axis, move_time + t, hst,
&p_pos_int, &p_pa_vel_int);
*pos_integral += a * p_pos_int;
*pa_velocity_integral += a * p_pa_vel_int;
}
}

struct pressure_advance_params {
Expand All @@ -142,6 +169,7 @@ typedef double (*pressure_advance_func)(

struct extruder_stepper {
struct stepper_kinematics sk;
struct shaper_pulses sp[3];
struct pressure_advance_params pa_params;
pressure_advance_func pa_func;
double time_offset, half_smooth_time, inv_half_smooth_time2;
Expand Down Expand Up @@ -197,16 +225,25 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m
struct coord e_pos, pa_vel;
double move_dist = move_get_distance(m, move_time);
for (i = 0; i < 3; ++i) {
int axis = 'x' + i;
struct shaper_pulses* sp = &es->sp[i];
int num_pulses = sp->num_pulses;
if (!hst) {
e_pos.axis[i] = m->axes_r.axis[i] * move_dist;
e_pos.axis[i] = num_pulses
? shaper_calc_position(m, axis, move_time, sp)
: m->start_pos.axis[i] + m->axes_r.axis[i] * move_dist;
pa_vel.axis[i] = 0.;
} else {
pa_range_integrate(m, 'x' + i, move_time, hst,
&e_pos.axis[i], &pa_vel.axis[i]);
if (num_pulses) {
shaper_pa_range_integrate(m, axis, move_time, hst, sp,
&e_pos.axis[i], &pa_vel.axis[i]);
} else {
pa_range_integrate(m, axis, move_time, hst,
&e_pos.axis[i], &pa_vel.axis[i]);
}
e_pos.axis[i] *= es->inv_half_smooth_time2;
pa_vel.axis[i] *= es->inv_half_smooth_time2;
}
e_pos.axis[i] += m->start_pos.axis[i];
}
double position = e_pos.x + e_pos.y + e_pos.z;
if (!hst)
Expand All @@ -220,6 +257,15 @@ static void
extruder_note_generation_time(struct extruder_stepper *es)
{
double pre_active = 0., post_active = 0.;
int i;
for (i = 0; i < 2; ++i) {
struct shaper_pulses* sp = &es->sp[i];
if (!es->sp[i].num_pulses) continue;
pre_active = sp->pulses[sp->num_pulses-1].t > pre_active
? sp->pulses[sp->num_pulses-1].t : pre_active;
post_active = -sp->pulses[0].t > post_active
? -sp->pulses[0].t : post_active;
}
pre_active += es->half_smooth_time + es->time_offset;
if (pre_active < 0.) pre_active = 0.;
post_active += es->half_smooth_time - es->time_offset;
Expand Down Expand Up @@ -256,6 +302,27 @@ extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk
es->pa_func = func;
}

int __visible
extruder_set_shaper_params(struct stepper_kinematics *sk, char axis
, int n, double a[], double t[])
{
if (axis != 'x' && axis != 'y')
return -1;
struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
struct shaper_pulses *sp = &es->sp[axis-'x'];
int status = init_shaper(n, a, t, sp);
extruder_note_generation_time(es);
return status;
}

double __visible
extruder_get_step_gen_window(struct stepper_kinematics *sk)
{
struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
return es->sk.gen_steps_pre_active > es->sk.gen_steps_post_active
? es->sk.gen_steps_pre_active : es->sk.gen_steps_post_active;
}

struct stepper_kinematics * __visible
extruder_stepper_alloc(void)
{
Expand Down
26 changes: 10 additions & 16 deletions klippy/chelper/kin_shaper.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,20 +11,14 @@
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
#include "kin_shaper.h" // struct shaper_pulses
#include "trapq.h" // struct move


/****************************************************************
* Shaper initialization
****************************************************************/

struct shaper_pulses {
int num_pulses;
struct {
double t, a;
} pulses[25];
};

// 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
Expand All @@ -38,7 +32,7 @@ shift_pulses(struct shaper_pulses *sp)
sp->pulses[i].t -= ts;
}

static int
int
init_shaper(int n, double a[], double t[], struct shaper_pulses *sp)
{
if (n < 0 || n > ARRAY_SIZE(sp->pulses)) {
Expand Down Expand Up @@ -89,9 +83,9 @@ get_axis_position_across_moves(struct move *m, int axis, double 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)
inline double
shaper_calc_position(struct move *m, int axis, double move_time
, struct shaper_pulses *sp)
{
double res = 0.;
int num_pulses = sp->num_pulses, i;
Expand Down Expand Up @@ -124,7 +118,7 @@ shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m
struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->sx.num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
}

Expand All @@ -136,7 +130,7 @@ shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m
struct input_shaper *is = container_of(sk, struct input_shaper, sk);
if (!is->sy.num_pulses)
return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time);
is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
}

Expand All @@ -150,9 +144,9 @@ shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m
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->sx.num_pulses)
is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx);
is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx);
if (is->sy.num_pulses)
is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy);
is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy);
return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T);
}

Expand Down Expand Up @@ -213,7 +207,7 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis
}

double __visible
input_shaper_get_step_generation_window(struct stepper_kinematics *sk)
input_shaper_get_step_gen_window(struct stepper_kinematics *sk)
{
struct input_shaper *is = container_of(sk, struct input_shaper, sk);
return is->sk.gen_steps_pre_active > is->sk.gen_steps_post_active
Expand Down
17 changes: 17 additions & 0 deletions klippy/chelper/kin_shaper.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
#ifndef __KIN_SHAPER_H
#define __KIN_SHAPER_H

struct shaper_pulses {
int num_pulses;
struct {
double t, a;
} pulses[5];
};

struct move;

int init_shaper(int n, double a[], double t[], struct shaper_pulses *sp);
double shaper_calc_position(struct move *m, int axis, double move_time
, struct shaper_pulses *sp);

#endif // kin_shaper.h
Loading

0 comments on commit a893f98

Please sign in to comment.