klipper/klippy/chelper/kin_extruder.c

146 lines
5.5 KiB
C

// Extruder stepper pulse time generation
//
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "compiler.h" // __visible
#include "itersolve.h" // struct stepper_kinematics
#include "pyhelper.h" // errorf
#include "trapq.h" // move_get_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
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))
static double
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 pressure_advance
, double base, 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
int can_pressure_advance = m->axes_r.y != 0.;
if (!can_pressure_advance)
pressure_advance = 0.;
base += 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 move_time
, double pressure_advance, double hst)
{
// Calculate integral for the current move
double res = 0., start = move_time - hst, end = move_time + hst;
double start_base = m->start_pos.x;
res += pa_move_integrate(m, pressure_advance, 0., start, move_time, start);
res -= pa_move_integrate(m, pressure_advance, 0., 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;
double base = prev->start_pos.x - start_base;
res += pa_move_integrate(prev, pressure_advance, base, 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);
double base = m->start_pos.x - start_base;
res -= pa_move_integrate(m, pressure_advance, base, 0., end, end);
}
return res;
}
struct extruder_stepper {
struct stepper_kinematics sk;
double pressure_advance, half_smooth_time, inv_half_smooth_time2;
};
static double
extruder_calc_position(struct stepper_kinematics *sk, struct move *m
, double move_time)
{
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);
// Apply pressure advance and average over smooth_time
double area = pa_range_integrate(m, move_time, es->pressure_advance, hst);
return m->start_pos.x + area * es->inv_half_smooth_time2;
}
void __visible
extruder_set_pressure_advance(struct stepper_kinematics *sk
, double pressure_advance, double smooth_time)
{
struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk);
double hst = smooth_time * .5;
es->half_smooth_time = hst;
es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst;
if (! hst)
return;
es->inv_half_smooth_time2 = 1. / (hst * hst);
es->pressure_advance = pressure_advance;
}
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;
}