trapq: Implement sentinel nodes on the trapq list
Use sentinels to make list traversal code simpler. Also add in null moves so that there are no time gaps in the list. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
2843c85019
commit
4dbe795ac2
|
@ -144,14 +144,12 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
|
||||||
{
|
{
|
||||||
double last_flush_time = sk->last_flush_time;
|
double last_flush_time = sk->last_flush_time;
|
||||||
sk->last_flush_time = flush_time;
|
sk->last_flush_time = flush_time;
|
||||||
if (!sk->tq || list_empty(&sk->tq->moves))
|
if (!sk->tq)
|
||||||
return 0;
|
return 0;
|
||||||
|
trapq_check_sentinels(sk->tq);
|
||||||
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
||||||
while (last_flush_time >= m->print_time + m->move_t) {
|
while (last_flush_time >= m->print_time + m->move_t)
|
||||||
if (list_is_last(&m->node, &sk->tq->moves))
|
|
||||||
return 0;
|
|
||||||
m = list_next_entry(m, node);
|
m = list_next_entry(m, node);
|
||||||
}
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
double start = m->print_time, end = start + m->move_t;
|
double start = m->print_time, end = start + m->move_t;
|
||||||
if (start < last_flush_time)
|
if (start < last_flush_time)
|
||||||
|
@ -166,7 +164,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
last_flush_time = end;
|
last_flush_time = end;
|
||||||
if (list_is_last(&m->node, &sk->tq->moves))
|
if (flush_time <= m->print_time + m->move_t)
|
||||||
return 0;
|
return 0;
|
||||||
m = list_next_entry(m, node);
|
m = list_next_entry(m, node);
|
||||||
}
|
}
|
||||||
|
@ -176,22 +174,19 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
|
||||||
double __visible
|
double __visible
|
||||||
itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
|
itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
|
||||||
{
|
{
|
||||||
if (!sk->tq || list_empty(&sk->tq->moves))
|
if (!sk->tq)
|
||||||
return 0.;
|
return 0.;
|
||||||
|
trapq_check_sentinels(sk->tq);
|
||||||
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
|
||||||
while (sk->last_flush_time >= m->print_time + m->move_t) {
|
while (sk->last_flush_time >= m->print_time + m->move_t)
|
||||||
if (list_is_last(&m->node, &sk->tq->moves))
|
|
||||||
return 0.;
|
|
||||||
m = list_next_entry(m, node);
|
m = list_next_entry(m, node);
|
||||||
}
|
for (;;) {
|
||||||
while (m->print_time < flush_time) {
|
|
||||||
if (check_active(sk, m))
|
if (check_active(sk, m))
|
||||||
return m->print_time;
|
return m->print_time;
|
||||||
if (list_is_last(&m->node, &sk->tq->moves))
|
if (flush_time <= m->print_time + m->move_t)
|
||||||
return 0.;
|
return 0.;
|
||||||
m = list_next_entry(m, node);
|
m = list_next_entry(m, node);
|
||||||
}
|
}
|
||||||
return 0.;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void __visible
|
void __visible
|
||||||
|
|
|
@ -86,6 +86,8 @@ move_get_coord(struct move *m, double move_time)
|
||||||
.z = m->start_pos.z + m->axes_r.z * move_dist };
|
.z = m->start_pos.z + m->axes_r.z * move_dist };
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define NEVER_TIME 9999999999999999.9
|
||||||
|
|
||||||
// Allocate a new 'trapq' object
|
// Allocate a new 'trapq' object
|
||||||
struct trapq * __visible
|
struct trapq * __visible
|
||||||
trapq_alloc(void)
|
trapq_alloc(void)
|
||||||
|
@ -93,6 +95,10 @@ trapq_alloc(void)
|
||||||
struct trapq *tq = malloc(sizeof(*tq));
|
struct trapq *tq = malloc(sizeof(*tq));
|
||||||
memset(tq, 0, sizeof(*tq));
|
memset(tq, 0, sizeof(*tq));
|
||||||
list_init(&tq->moves);
|
list_init(&tq->moves);
|
||||||
|
struct move *head_sentinel = move_alloc(), *tail_sentinel = move_alloc();
|
||||||
|
tail_sentinel->print_time = tail_sentinel->move_t = NEVER_TIME;
|
||||||
|
list_add_head(&head_sentinel->node, &tq->moves);
|
||||||
|
list_add_tail(&tail_sentinel->node, &tq->moves);
|
||||||
return tq;
|
return tq;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -108,19 +114,55 @@ trapq_free(struct trapq *tq)
|
||||||
free(tq);
|
free(tq);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Update the list sentinels
|
||||||
|
void
|
||||||
|
trapq_check_sentinels(struct trapq *tq)
|
||||||
|
{
|
||||||
|
struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
|
||||||
|
if (tail_sentinel->print_time)
|
||||||
|
// Already up to date
|
||||||
|
return;
|
||||||
|
struct move *m = list_prev_entry(tail_sentinel, node);
|
||||||
|
struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
|
||||||
|
if (m == head_sentinel) {
|
||||||
|
// No moves at all on this list
|
||||||
|
tail_sentinel->print_time = NEVER_TIME;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
tail_sentinel->print_time = m->print_time + m->move_t;
|
||||||
|
tail_sentinel->start_pos = move_get_coord(m, m->move_t);
|
||||||
|
}
|
||||||
|
|
||||||
// Add a move to the trapezoid velocity queue
|
// Add a move to the trapezoid velocity queue
|
||||||
void
|
void
|
||||||
trapq_add_move(struct trapq *tq, struct move *m)
|
trapq_add_move(struct trapq *tq, struct move *m)
|
||||||
{
|
{
|
||||||
list_add_tail(&m->node, &tq->moves);
|
struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
|
||||||
|
struct move *prev = list_prev_entry(tail_sentinel, node);
|
||||||
|
if (prev->print_time + prev->move_t < m->print_time) {
|
||||||
|
// Add a null move to fill time gap
|
||||||
|
struct move *null_move = move_alloc();
|
||||||
|
null_move->start_pos = m->start_pos;
|
||||||
|
null_move->print_time = prev->print_time + prev->move_t;
|
||||||
|
null_move->move_t = m->print_time - null_move->print_time;
|
||||||
|
list_add_before(&null_move->node, &tail_sentinel->node);
|
||||||
|
}
|
||||||
|
list_add_before(&m->node, &tail_sentinel->node);
|
||||||
|
tail_sentinel->print_time = 0.;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Free any moves older than `print_time` from the trapezoid velocity queue
|
// Free any moves older than `print_time` from the trapezoid velocity queue
|
||||||
void __visible
|
void __visible
|
||||||
trapq_free_moves(struct trapq *tq, double print_time)
|
trapq_free_moves(struct trapq *tq, double print_time)
|
||||||
{
|
{
|
||||||
while (!list_empty(&tq->moves)) {
|
struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
|
||||||
struct move *m = list_first_entry(&tq->moves, struct move, node);
|
struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
|
||||||
|
for (;;) {
|
||||||
|
struct move *m = list_next_entry(head_sentinel, node);
|
||||||
|
if (m == tail_sentinel) {
|
||||||
|
tail_sentinel->print_time = NEVER_TIME;
|
||||||
|
return;
|
||||||
|
}
|
||||||
if (m->print_time + m->move_t > print_time)
|
if (m->print_time + m->move_t > print_time)
|
||||||
return;
|
return;
|
||||||
list_del(&m->node);
|
list_del(&m->node);
|
||||||
|
|
|
@ -29,6 +29,7 @@ double move_get_distance(struct move *m, double move_time);
|
||||||
struct coord move_get_coord(struct move *m, double move_time);
|
struct coord move_get_coord(struct move *m, double move_time);
|
||||||
struct trapq *trapq_alloc(void);
|
struct trapq *trapq_alloc(void);
|
||||||
void trapq_free(struct trapq *tq);
|
void trapq_free(struct trapq *tq);
|
||||||
|
void trapq_check_sentinels(struct trapq *tq);
|
||||||
void trapq_add_move(struct trapq *tq, struct move *m);
|
void trapq_add_move(struct trapq *tq, struct move *m);
|
||||||
void trapq_free_moves(struct trapq *tq, double print_time);
|
void trapq_free_moves(struct trapq *tq, double print_time);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue