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

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

View File

@ -12,6 +12,71 @@
#include "pyhelper.h" // errorf
#include "trapq.h" // move_get_distance
// 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
static 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
static 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;
}
}
}
struct extruder_stepper {
struct stepper_kinematics sk;
double pressure_advance_factor, half_smooth_time, inv_smooth_time;

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

View File

@ -32,8 +32,6 @@ 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 move *trapq_find_move(struct move *m, double *ptime);
struct trapq *trapq_alloc(void);
void trapq_free(struct trapq *tq);
void trapq_check_sentinels(struct trapq *tq);