-
-
Notifications
You must be signed in to change notification settings - Fork 5.3k
/
kin_extruder.c
139 lines (129 loc) · 5.1 KB
/
kin_extruder.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
// 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 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
double pressure_advance = m->axes_r.y;
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 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, 0., start, move_time, start);
res -= pa_move_integrate(m, 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, 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, base, 0., end, end);
}
return res;
}
struct extruder_stepper {
struct stepper_kinematics sk;
double 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, hst);
return m->start_pos.x + area * es->inv_half_smooth_time2;
}
void __visible
extruder_set_smooth_time(struct stepper_kinematics *sk, 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);
}
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;
}