trapq: Keep history of recent trapq moves

Store trapq moves in a separate "history" list after each move is
nominally expired.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-07-19 11:32:16 -04:00
parent e2f483aea3
commit 45c232b2c7
3 changed files with 71 additions and 4 deletions

View File

@ -68,6 +68,13 @@ defs_itersolve = """
"""
defs_trapq = """
struct pull_move {
double print_time, move_t;
double start_v, accel;
double start_x, start_y, start_z;
double x_r, y_r, z_r;
};
void trapq_append(struct trapq *tq, double print_time
, double accel_t, double cruise_t, double decel_t
, double start_pos_x, double start_pos_y, double start_pos_z
@ -76,6 +83,8 @@ defs_trapq = """
struct trapq *trapq_alloc(void);
void trapq_free(struct trapq *tq);
void trapq_finalize_moves(struct trapq *tq, double print_time);
int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
, double start_time, double end_time);
"""
defs_kin_cartesian = """

View File

@ -1,6 +1,6 @@
// Trapezoidal velocity movement queue
//
// Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
// Copyright (C) 2018-2021 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
@ -95,6 +95,7 @@ trapq_alloc(void)
struct trapq *tq = malloc(sizeof(*tq));
memset(tq, 0, sizeof(*tq));
list_init(&tq->moves);
list_init(&tq->history);
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);
@ -111,6 +112,11 @@ trapq_free(struct trapq *tq)
list_del(&m->node);
free(m);
}
while (!list_empty(&tq->history)) {
struct move *m = list_first_entry(&tq->history, struct move, node);
list_del(&m->node);
free(m);
}
free(tq);
}
@ -157,21 +163,64 @@ trapq_add_move(struct trapq *tq, struct move *m)
tail_sentinel->print_time = 0.;
}
#define HISTORY_EXPIRE (30.0)
// Expire any moves older than `print_time` from the trapezoid velocity queue
void __visible
trapq_finalize_moves(struct trapq *tq, double print_time)
{
struct move *head_sentinel = list_first_entry(&tq->moves, struct move,node);
struct move *tail_sentinel = list_last_entry(&tq->moves, struct move, node);
// Move expired moves from main "moves" list to "history" list
for (;;) {
struct move *m = list_next_entry(head_sentinel, node);
if (m == tail_sentinel) {
tail_sentinel->print_time = NEVER_TIME;
return;
break;
}
if (m->print_time + m->move_t > print_time)
return;
break;
list_del(&m->node);
list_add_head(&m->node, &tq->history);
}
// Free old moves from history list
if (list_empty(&tq->history))
return;
struct move *latest = list_first_entry(&tq->history, struct move, node);
double expire_time = latest->print_time + latest->move_t - HISTORY_EXPIRE;
for (;;) {
struct move *m = list_last_entry(&tq->history, struct move, node);
if (m == latest || m->print_time + m->move_t > expire_time)
break;
list_del(&m->node);
free(m);
}
}
// Return history of movement queue
int __visible
trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
, double start_time, double end_time)
{
int res = 0;
struct move *m;
list_for_each_entry(m, &tq->history, node) {
if (start_time >= m->print_time + m->move_t || res >= max)
break;
if (end_time <= m->print_time)
continue;
p->print_time = m->print_time;
p->move_t = m->move_t;
p->start_v = m->start_v;
p->accel = 2. * m->half_accel;
p->start_x = m->start_pos.x;
p->start_y = m->start_pos.y;
p->start_z = m->start_pos.z;
p->x_r = m->axes_r.x;
p->y_r = m->axes_r.y;
p->z_r = m->axes_r.z;
p++;
res++;
}
return res;
}

View File

@ -21,7 +21,14 @@ struct move {
};
struct trapq {
struct list_head moves;
struct list_head moves, history;
};
struct pull_move {
double print_time, move_t;
double start_v, accel;
double start_x, start_y, start_z;
double x_r, y_r, z_r;
};
struct move *move_alloc(void);
@ -37,5 +44,7 @@ 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_finalize_moves(struct trapq *tq, double print_time);
int trapq_extract_old(struct trapq *tq, struct pull_move *p, int max
, double start_time, double end_time);
#endif // trapq.h