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

trapq: Add code to calculate definitive integral 

Support calculating the definitive integral of a cartesian axis
position over a time range of the movement queue.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2019-11-01 22:53:00 -04:00
parent d00023f3bb
commit a28b29993a
2 changed files with 51 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
klippy/chelper/trapq.c
View File

@ -86,6 +86,50 @@ 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;
}
#define NEVER_TIME 9999999999999999.9
// Allocate a new 'trapq' object

6
klippy/chelper/trapq.h
View File

@ -4,7 +4,12 @@
#include "list.h" // list_node
struct coord {
union {
struct {
double x, y, z;
};
double axis[3];
};
};
struct move {
@ -27,6 +32,7 @@ void trapq_append(struct trapq *tq, double print_time
, double start_v, double cruise_v, double accel);
double move_get_distance(struct move *m, double move_time);
struct coord move_get_coord(struct move *m, double move_time);
double trapq_integrate(struct move *m, int axis, double start, double end);
struct trapq *trapq_alloc(void);
void trapq_free(struct trapq *tq);
void trapq_check_sentinels(struct trapq *tq);