3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

trapq: Minor code movement in trapq.c 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2023-01-07 14:13:00 -05:00
parent 01f90e8f28
commit 4ea11dbb4f
2 changed files with 53 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
klippy/chelper/trapq.c
View File

@ -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

10
klippy/chelper/trapq.h
View File

@ -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);