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:
Kevin O'Connor 2019-11-01 18:32:06 -04:00
parent 2843c85019
commit 4dbe795ac2
3 changed files with 55 additions and 17 deletions

View File

@ -144,14 +144,12 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
{
double last_flush_time = sk->last_flush_time;
sk->last_flush_time = flush_time;
if (!sk->tq || list_empty(&sk->tq->moves))
if (!sk->tq)
return 0;
trapq_check_sentinels(sk->tq);
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
while (last_flush_time >= m->print_time + m->move_t) {
if (list_is_last(&m->node, &sk->tq->moves))
return 0;
while (last_flush_time >= m->print_time + m->move_t)
m = list_next_entry(m, node);
}
for (;;) {
double start = m->print_time, end = start + m->move_t;
if (start < last_flush_time)
@ -166,7 +164,7 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
return ret;
}
last_flush_time = end;
if (list_is_last(&m->node, &sk->tq->moves))
if (flush_time <= m->print_time + m->move_t)
return 0;
m = list_next_entry(m, node);
}
@ -176,22 +174,19 @@ itersolve_generate_steps(struct stepper_kinematics *sk, double flush_time)
double __visible
itersolve_check_active(struct stepper_kinematics *sk, double flush_time)
{
if (!sk->tq || list_empty(&sk->tq->moves))
if (!sk->tq)
return 0.;
trapq_check_sentinels(sk->tq);
struct move *m = list_first_entry(&sk->tq->moves, struct move, node);
while (sk->last_flush_time >= m->print_time + m->move_t) {
if (list_is_last(&m->node, &sk->tq->moves))
return 0.;
while (sk->last_flush_time >= m->print_time + m->move_t)
m = list_next_entry(m, node);
}
while (m->print_time < flush_time) {
for (;;) {
if (check_active(sk, m))
return m->print_time;
if (list_is_last(&m->node, &sk->tq->moves))
if (flush_time <= m->print_time + m->move_t)
return 0.;
m = list_next_entry(m, node);
}
return 0.;
}
void __visible

View File

@ -86,6 +86,8 @@ move_get_coord(struct move *m, double move_time)
.z = m->start_pos.z + m->axes_r.z * move_dist };
}
#define NEVER_TIME 9999999999999999.9
// Allocate a new 'trapq' object
struct trapq * __visible
trapq_alloc(void)
@ -93,6 +95,10 @@ trapq_alloc(void)
struct trapq *tq = malloc(sizeof(*tq));
memset(tq, 0, sizeof(*tq));
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;
}
@ -108,19 +114,55 @@ trapq_free(struct trapq *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
void
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
void __visible
trapq_free_moves(struct trapq *tq, double print_time)
{
while (!list_empty(&tq->moves)) {
struct move *m = list_first_entry(&tq->moves, struct move, node);
struct move *head_sentinel = 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)
return;
list_del(&m->node);

View File

@ -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 trapq *trapq_alloc(void);
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_free_moves(struct trapq *tq, double print_time);