From 4ea11dbb4f2f6c316c241aaeac72dbff14893406 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sat, 7 Jan 2023 14:13:00 -0500 Subject: [PATCH] trapq: Minor code movement in trapq.c Signed-off-by: Kevin O'Connor --- klippy/chelper/trapq.c | 96 +++++++++++++++++++++--------------------- klippy/chelper/trapq.h | 10 ++--- 2 files changed, 53 insertions(+), 53 deletions(-) diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index dbb72865..9b1b501b 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -20,54 +20,6 @@ move_alloc(void) return m; } -// Fill and add a move to the trapezoid velocity queue -void __visible -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 - , double axes_r_x, double axes_r_y, double axes_r_z - , double start_v, double cruise_v, double accel) -{ - struct coord start_pos = { .x=start_pos_x, .y=start_pos_y, .z=start_pos_z }; - struct coord axes_r = { .x=axes_r_x, .y=axes_r_y, .z=axes_r_z }; - if (accel_t) { - struct move *m = move_alloc(); - m->print_time = print_time; - m->move_t = accel_t; - m->start_v = start_v; - m->half_accel = .5 * accel; - m->start_pos = start_pos; - m->axes_r = axes_r; - trapq_add_move(tq, m); - - print_time += accel_t; - start_pos = move_get_coord(m, accel_t); - } - if (cruise_t) { - struct move *m = move_alloc(); - m->print_time = print_time; - m->move_t = cruise_t; - m->start_v = cruise_v; - m->half_accel = 0.; - m->start_pos = start_pos; - m->axes_r = axes_r; - trapq_add_move(tq, m); - - print_time += cruise_t; - start_pos = move_get_coord(m, cruise_t); - } - if (decel_t) { - struct move *m = move_alloc(); - m->print_time = print_time; - m->move_t = decel_t; - m->start_v = cruise_v; - m->half_accel = -.5 * accel; - m->start_pos = start_pos; - m->axes_r = axes_r; - trapq_add_move(tq, m); - } -} - // Return the distance moved given a time in a move inline double move_get_distance(struct move *m, double move_time) @@ -163,6 +115,54 @@ trapq_add_move(struct trapq *tq, struct move *m) tail_sentinel->print_time = 0.; } +// Fill and add a move to the trapezoid velocity queue +void __visible +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 + , double axes_r_x, double axes_r_y, double axes_r_z + , double start_v, double cruise_v, double accel) +{ + struct coord start_pos = { .x=start_pos_x, .y=start_pos_y, .z=start_pos_z }; + struct coord axes_r = { .x=axes_r_x, .y=axes_r_y, .z=axes_r_z }; + if (accel_t) { + struct move *m = move_alloc(); + m->print_time = print_time; + m->move_t = accel_t; + m->start_v = start_v; + m->half_accel = .5 * accel; + m->start_pos = start_pos; + m->axes_r = axes_r; + trapq_add_move(tq, m); + + print_time += accel_t; + start_pos = move_get_coord(m, accel_t); + } + if (cruise_t) { + struct move *m = move_alloc(); + m->print_time = print_time; + m->move_t = cruise_t; + m->start_v = cruise_v; + m->half_accel = 0.; + m->start_pos = start_pos; + m->axes_r = axes_r; + trapq_add_move(tq, m); + + print_time += cruise_t; + start_pos = move_get_coord(m, cruise_t); + } + if (decel_t) { + struct move *m = move_alloc(); + m->print_time = print_time; + m->move_t = decel_t; + m->start_v = cruise_v; + m->half_accel = -.5 * accel; + m->start_pos = start_pos; + m->axes_r = axes_r; + trapq_add_move(tq, m); + } +} + #define HISTORY_EXPIRE (30.0) // Expire any moves older than `print_time` from the trapezoid velocity queue diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index dbde8d3e..bd8f4e8c 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -32,17 +32,17 @@ struct pull_move { }; struct move *move_alloc(void); -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 - , double axes_r_x, double axes_r_y, double axes_r_z - , 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); 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_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 + , double axes_r_x, double axes_r_y, double axes_r_z + , double start_v, double cruise_v, double accel); void trapq_finalize_moves(struct trapq *tq, double print_time); void trapq_set_position(struct trapq *tq, double print_time , double pos_x, double pos_y, double pos_z);