kin_extruder: Convert pressure advance to use "weighted average"

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2019-12-14 10:30:25 -05:00
parent 54149e38f9
commit dabffcc22c
4 changed files with 66 additions and 27 deletions

View File

@ -284,8 +284,8 @@ rate, the more filament must be pushed in during acceleration to
account for pressure. During head deceleration the extra filament is
retracted (the extruder will have a negative velocity).
The "smoothing" is implemented by averaging the extruder position over
a small time period (as specified by the
The "smoothing" is implemented using a weighted average of the
extruder position over a small time period (as specified by the
`pressure_advance_smooth_time` config parameter). This averaging can
span multiple g-code moves. Note how the extruder motor will start
moving prior to the nominal start of the first extrusion move and will
@ -294,6 +294,7 @@ continue to move after the nominal end of the last extrusion move.
Key formula for "smoothed pressure advance":
```
smooth_pa_position(t) =
( definitive_integral(pa_position, from=t-smooth_time/2, to=t+smooth_time/2)
/ smooth_time )
( definitive_integral(pa_position(x) * (smooth_time/2 - abs(t - x)) * dx,
from=t-smooth_time/2, to=t+smooth_time/2)
/ (smooth_time/2)^2 )
```

Binary file not shown.

Before

Width:  |  Height:  |  Size: 24 KiB

After

Width:  |  Height:  |  Size: 24 KiB

View File

@ -12,55 +12,90 @@
#include "pyhelper.h" // errorf
#include "trapq.h" // move_get_distance
// Calculate the definitive integral of the move distance
// Without pressure advance, the extruder stepper position is:
// extruder_position(t) = nominal_position(t)
// When pressure advance is enabled, additional filament is pushed
// into the extruder during acceleration (and retracted during
// deceleration). The formula is:
// pa_position(t) = (nominal_position(t)
// + pressure_advance * nominal_velocity(t))
// Which is then "smoothed" using a weighted average:
// smooth_position(t) = (
// definitive_integral(pa_position(x) * (smooth_time/2 - abs(t-x)) * dx,
// from=t-smooth_time/2, to=t+smooth_time/2)
// / ((smooth_time/2)**2))
// Calculate the definitive integral of the motion formula:
// position(t) = base + t * (start_v + t * half_accel)
static double
move_integrate_distance(struct move *m, double start, double end)
extruder_integrate(double base, double start_v, double half_accel
, double start, double end)
{
double half_v = .5 * m->start_v, sixth_a = (1. / 3.) * m->half_accel;
double si = start * start * (half_v + sixth_a * start);
double ei = end * end * (half_v + sixth_a * end);
double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel;
double si = start * (base + start * (half_v + start * sixth_a));
double ei = end * (base + end * (half_v + end * sixth_a));
return ei - si;
}
// Calculate the definitive integral of extruder with pressure advance
// Calculate the definitive integral of time weighted position:
// weighted_position(t) = t * (base + t * (start_v + t * half_accel))
static double
pa_move_integrate(struct move *m, double start, double end)
extruder_integrate_time(double base, double start_v, double half_accel
, double start, double end)
{
double half_b = .5 * base, third_v = (1. / 3.) * start_v;
double eighth_a = .25 * half_accel;
double si = start * start * (half_b + start * (third_v + start * eighth_a));
double ei = end * end * (half_b + end * (third_v + end * eighth_a));
return ei - si;
}
// Calculate the definitive integral of extruder for a given move
static double
pa_move_integrate(struct move *m, double start, double end, double time_offset)
{
if (start < 0.)
start = 0.;
if (end > m->move_t)
end = m->move_t;
// Calculate base position and velocity with pressure advance
double pressure_advance = m->axes_r.y;
double avg_v = m->start_v + (start + end) * m->half_accel;
double pa_add = pressure_advance * avg_v;
double base = (m->start_pos.x + pa_add) * (end - start);
return base + move_integrate_distance(m, start, end);
double base = m->start_pos.x + pressure_advance * m->start_v;
double start_v = m->start_v + pressure_advance * 2. * m->half_accel;
// Calculate definitive integral
double ha = m->half_accel;
double iext = extruder_integrate(base, start_v, ha, start, end);
double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end);
return wgt_ext - time_offset * iext;
}
// Calculate the definitive integral of the extruder over a range of moves
static double
pa_range_integrate(struct move *m, double start, double end)
pa_range_integrate(struct move *m, double move_time, double hst)
{
double res = pa_move_integrate(m, start, end);
// Calculate integral for the current move
double res = 0., start = move_time - hst, end = move_time + hst;
res += pa_move_integrate(m, start, move_time, start);
res -= pa_move_integrate(m, move_time, end, end);
// Integrate over previous moves
struct move *prev = m;
while (unlikely(start < 0.)) {
prev = list_prev_entry(prev, node);
start += prev->move_t;
res += pa_move_integrate(prev, start, prev->move_t);
res += pa_move_integrate(prev, start, prev->move_t, start);
}
// Integrate over future moves
while (unlikely(end > m->move_t)) {
end -= m->move_t;
m = list_next_entry(m, node);
res += pa_move_integrate(m, 0., end);
res -= pa_move_integrate(m, 0., end, end);
}
return res;
}
struct extruder_stepper {
struct stepper_kinematics sk;
double half_smooth_time, inv_smooth_time;
double half_smooth_time, inv_half_smooth_time2;
};
static double
@ -73,8 +108,8 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m
// Pressure advance not enabled
return m->start_pos.x + move_get_distance(m, move_time);
// Apply pressure advance and average over smooth_time
double area = pa_range_integrate(m, move_time - hst, move_time + hst);
return area * es->inv_smooth_time;
double area = pa_range_integrate(m, move_time, hst);
return area * es->inv_half_smooth_time2;
}
void __visible
@ -86,7 +121,7 @@ extruder_set_smooth_time(struct stepper_kinematics *sk, double smooth_time)
es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
if (! hst)
return;
es->inv_smooth_time = 1. / smooth_time;
es->inv_half_smooth_time2 = 1. / (hst * hst);
}
struct stepper_kinematics * __visible

View File

@ -67,13 +67,16 @@ def calc_pa_raw(t, positions):
i = time_to_index(t)
return positions[i] + pa * (positions[i+1] - positions[i])
def calc_pa_smooth(t, positions):
def calc_pa_weighted(t, positions):
base_index = time_to_index(t)
start_index = time_to_index(t - PA_HALF_SMOOTH_T) + 1
end_index = time_to_index(t + PA_HALF_SMOOTH_T)
diff = .5 * (end_index - start_index)
pa = PRESSURE_ADVANCE * INV_SEG_TIME
pa_data = [positions[i] + pa * (positions[i+1] - positions[i])
pa_data = [(positions[i] + pa * (positions[i+1] - positions[i]))
* (diff - abs(i-base_index))
for i in range(start_index, end_index)]
return sum(pa_data) / (end_index - start_index)
return sum(pa_data) / diff**2
######################################################################
@ -92,7 +95,7 @@ def plot_motion():
pa_positions = [calc_pa_raw(t, positions) for t in times]
pa_velocities = gen_deriv(pa_positions)
# Smoothed motion
sm_positions = [calc_pa_smooth(t, positions) for t in times]
sm_positions = [calc_pa_weighted(t, positions) for t in times]
sm_velocities = gen_deriv(sm_positions)
# Build plot
shift_times = [t - MARGIN_TIME for t in times]