h3n3/klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

kin_extruder: Move integration code from trapq.c to kin_extruder.c

Browse Source 
Move the code that calculates the definitive integral to the
kin_extruder.c file.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
2019-12-08 18:30:42 -05:00
parent 8b75bddc0f
commit 730a6d868b
3 changed files with 65 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
klippy/chelper/trapq.c
View File

@@ -86,71 +86,6 @@ move_get_coord(struct move *m, double move_time)
.z = m->start_pos.z + m->axes_r.z * move_dist };
}
// Helper code for integrating acceleration
static double
integrate_accel(struct move *m, 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);
return ei - si;
}
// Calculate the definitive integral on part of a move
static double
move_integrate(struct move *m, int axis, double start, double end)
{
if (start < 0.)
start = 0.;
if (end > m->move_t)
end = m->move_t;
double base = m->start_pos.axis[axis - 'x'] * (end - start);
double integral = integrate_accel(m, start, end);
return base + integral * m->axes_r.axis[axis - 'x'];
}
// Calculate the definitive integral for a cartesian axis
double
trapq_integrate(struct move *m, int axis, double start, double end)
{
double res = move_integrate(m, axis, start, end);
// Integrate over previous moves
struct move *prev = m;
while (unlikely(start < 0.)) {
prev = list_prev_entry(prev, node);
start += prev->move_t;
res += move_integrate(prev, axis, start, prev->move_t);
}
// Integrate over future moves
while (unlikely(end > m->move_t)) {
end -= m->move_t;
m = list_next_entry(m, node);
res += move_integrate(m, axis, 0., end);
}
return res;
}
// Find a move associated with a given time
struct move *
trapq_find_move(struct move *m, double *ptime)
{
double move_time = *ptime;
for (;;) {
if (unlikely(move_time < 0.)) {
// Check previous move in list
m = list_prev_entry(m, node);
move_time += m->move_t;
} else if (unlikely(move_time > m->move_t)) {
// Check next move in list
move_time -= m->move_t;
m = list_next_entry(m, node);
} else {
*ptime = move_time;
return m;
}
}
}
#define NEVER_TIME 9999999999999999.9
// Allocate a new 'trapq' object