2018-06-13 23:53:25 +03:00
|
|
|
// Extruder stepper pulse time generation
|
|
|
|
//
|
2019-10-29 17:57:36 +03:00
|
|
|
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
|
2018-06-13 23:53:25 +03:00
|
|
|
//
|
|
|
|
// This file may be distributed under the terms of the GNU GPLv3 license.
|
|
|
|
|
2019-11-06 00:21:06 +03:00
|
|
|
#include <stddef.h> // offsetof
|
2018-06-13 23:53:25 +03:00
|
|
|
#include <stdlib.h> // malloc
|
|
|
|
#include <string.h> // memset
|
|
|
|
#include "compiler.h" // __visible
|
|
|
|
#include "itersolve.h" // struct stepper_kinematics
|
|
|
|
#include "pyhelper.h" // errorf
|
2019-10-27 20:23:03 +03:00
|
|
|
#include "trapq.h" // move_get_distance
|
2018-06-13 23:53:25 +03:00
|
|
|
|
2019-12-14 18:30:25 +03:00
|
|
|
// 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
|
|
|
|
extruder_integrate(double base, double start_v, double half_accel
|
|
|
|
, double start, double 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 time weighted position:
|
|
|
|
// weighted_position(t) = t * (base + t * (start_v + t * half_accel))
|
2019-12-09 02:30:42 +03:00
|
|
|
static double
|
2019-12-14 18:30:25 +03:00
|
|
|
extruder_integrate_time(double base, double start_v, double half_accel
|
|
|
|
, double start, double end)
|
2019-12-09 02:30:42 +03:00
|
|
|
{
|
2019-12-14 18:30:25 +03:00
|
|
|
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));
|
2019-12-09 02:30:42 +03:00
|
|
|
return ei - si;
|
|
|
|
}
|
|
|
|
|
2019-12-14 18:30:25 +03:00
|
|
|
// Calculate the definitive integral of extruder for a given move
|
2019-12-09 02:30:42 +03:00
|
|
|
static double
|
2022-01-11 21:21:07 +03:00
|
|
|
pa_move_integrate(struct move *m, double pressure_advance
|
|
|
|
, double base, double start, double end, double time_offset)
|
2019-12-09 02:30:42 +03:00
|
|
|
{
|
|
|
|
if (start < 0.)
|
|
|
|
start = 0.;
|
|
|
|
if (end > m->move_t)
|
|
|
|
end = m->move_t;
|
2019-12-14 18:30:25 +03:00
|
|
|
// Calculate base position and velocity with pressure advance
|
2022-01-11 21:21:07 +03:00
|
|
|
int can_pressure_advance = m->axes_r.y != 0.;
|
|
|
|
if (!can_pressure_advance)
|
|
|
|
pressure_advance = 0.;
|
2020-10-24 05:59:20 +03:00
|
|
|
base += pressure_advance * m->start_v;
|
2019-12-14 18:30:25 +03:00
|
|
|
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;
|
2019-12-09 02:30:42 +03:00
|
|
|
}
|
|
|
|
|
2019-12-09 03:54:36 +03:00
|
|
|
// Calculate the definitive integral of the extruder over a range of moves
|
2019-12-09 02:30:42 +03:00
|
|
|
static double
|
2022-01-11 21:21:07 +03:00
|
|
|
pa_range_integrate(struct move *m, double move_time
|
|
|
|
, double pressure_advance, double hst)
|
2019-12-09 02:30:42 +03:00
|
|
|
{
|
2019-12-14 18:30:25 +03:00
|
|
|
// Calculate integral for the current move
|
|
|
|
double res = 0., start = move_time - hst, end = move_time + hst;
|
2020-10-24 05:59:20 +03:00
|
|
|
double start_base = m->start_pos.x;
|
2022-01-11 21:21:07 +03:00
|
|
|
res += pa_move_integrate(m, pressure_advance, 0., start, move_time, start);
|
|
|
|
res -= pa_move_integrate(m, pressure_advance, 0., move_time, end, end);
|
2019-12-09 02:30:42 +03:00
|
|
|
// Integrate over previous moves
|
|
|
|
struct move *prev = m;
|
|
|
|
while (unlikely(start < 0.)) {
|
|
|
|
prev = list_prev_entry(prev, node);
|
|
|
|
start += prev->move_t;
|
2020-10-24 05:59:20 +03:00
|
|
|
double base = prev->start_pos.x - start_base;
|
2022-01-11 21:21:07 +03:00
|
|
|
res += pa_move_integrate(prev, pressure_advance, base, start
|
|
|
|
, prev->move_t, start);
|
2019-12-09 02:30:42 +03:00
|
|
|
}
|
|
|
|
// Integrate over future moves
|
|
|
|
while (unlikely(end > m->move_t)) {
|
|
|
|
end -= m->move_t;
|
|
|
|
m = list_next_entry(m, node);
|
2020-10-24 05:59:20 +03:00
|
|
|
double base = m->start_pos.x - start_base;
|
2022-01-11 21:21:07 +03:00
|
|
|
res -= pa_move_integrate(m, pressure_advance, base, 0., end, end);
|
2019-12-09 02:30:42 +03:00
|
|
|
}
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
2019-11-06 00:21:06 +03:00
|
|
|
struct extruder_stepper {
|
|
|
|
struct stepper_kinematics sk;
|
2022-01-11 21:21:07 +03:00
|
|
|
double pressure_advance, half_smooth_time, inv_half_smooth_time2;
|
2019-11-06 00:21:06 +03:00
|
|
|
};
|
|
|
|
|
2018-06-13 23:53:25 +03:00
|
|
|
static double
|
|
|
|
extruder_calc_position(struct stepper_kinematics *sk, struct move *m
|
|
|
|
, double move_time)
|
|
|
|
{
|
2019-11-06 00:21:06 +03:00
|
|
|
struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
|
|
|
|
double hst = es->half_smooth_time;
|
|
|
|
if (!hst)
|
|
|
|
// Pressure advance not enabled
|
|
|
|
return m->start_pos.x + move_get_distance(m, move_time);
|
2019-12-09 03:54:36 +03:00
|
|
|
// Apply pressure advance and average over smooth_time
|
2022-01-11 21:21:07 +03:00
|
|
|
double area = pa_range_integrate(m, move_time, es->pressure_advance, hst);
|
2020-10-24 05:59:20 +03:00
|
|
|
return m->start_pos.x + area * es->inv_half_smooth_time2;
|
2018-06-13 23:53:25 +03:00
|
|
|
}
|
|
|
|
|
|
|
|
void __visible
|
2022-01-11 21:21:07 +03:00
|
|
|
extruder_set_pressure_advance(struct stepper_kinematics *sk
|
|
|
|
, double pressure_advance, double smooth_time)
|
2018-06-13 23:53:25 +03:00
|
|
|
{
|
2019-11-06 00:21:06 +03:00
|
|
|
struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
|
2019-12-09 03:54:36 +03:00
|
|
|
double hst = smooth_time * .5;
|
2019-12-10 23:53:50 +03:00
|
|
|
es->half_smooth_time = hst;
|
|
|
|
es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
|
2019-12-09 03:54:36 +03:00
|
|
|
if (! hst)
|
2019-11-06 00:21:06 +03:00
|
|
|
return;
|
2019-12-14 18:30:25 +03:00
|
|
|
es->inv_half_smooth_time2 = 1. / (hst * hst);
|
2022-01-11 21:21:07 +03:00
|
|
|
es->pressure_advance = pressure_advance;
|
2019-11-06 00:21:06 +03:00
|
|
|
}
|
2018-06-13 23:53:25 +03:00
|
|
|
|
2019-11-06 00:21:06 +03:00
|
|
|
struct stepper_kinematics * __visible
|
|
|
|
extruder_stepper_alloc(void)
|
|
|
|
{
|
|
|
|
struct extruder_stepper *es = malloc(sizeof(*es));
|
|
|
|
memset(es, 0, sizeof(*es));
|
|
|
|
es->sk.calc_position_cb = extruder_calc_position;
|
|
|
|
es->sk.active_flags = AF_X;
|
|
|
|
return &es->sk;
|
2018-06-13 23:53:25 +03:00
|
|
|
}
|