mirror of https://github.com/Desuuuu/klipper.git
trapq: Use separate 'move' entries for accel, cruise, and decel phases
Only track a single acceleration movement in a 'struct move' instance. Break the classic trapezoid movement (accel, cruise, decel) into three separate movements. This simplifies the calculation logic. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
7ca86f1723
commit
076a66f791
|
@ -93,7 +93,7 @@ defs_kin_winch = """
|
||||||
defs_kin_extruder = """
|
defs_kin_extruder = """
|
||||||
struct stepper_kinematics *extruder_stepper_alloc(void);
|
struct stepper_kinematics *extruder_stepper_alloc(void);
|
||||||
void extruder_add_move(struct trapq *tq, double print_time
|
void extruder_add_move(struct trapq *tq, double print_time
|
||||||
, double accel_t, double cruise_t, double decel_t, double start_pos
|
, double accel_t, double cruise_t, double decel_t, double start_e_pos
|
||||||
, double start_v, double cruise_v, double accel
|
, double start_v, double cruise_v, double accel
|
||||||
, double extra_accel_v, double extra_decel_v);
|
, double extra_accel_v, double extra_decel_v);
|
||||||
"""
|
"""
|
||||||
|
|
|
@ -32,30 +32,49 @@ extruder_stepper_alloc(void)
|
||||||
void __visible
|
void __visible
|
||||||
extruder_add_move(struct trapq *tq, double print_time
|
extruder_add_move(struct trapq *tq, double print_time
|
||||||
, double accel_t, double cruise_t, double decel_t
|
, double accel_t, double cruise_t, double decel_t
|
||||||
, double start_pos
|
, double start_e_pos
|
||||||
, double start_v, double cruise_v, double accel
|
, double start_v, double cruise_v, double accel
|
||||||
, double extra_accel_v, double extra_decel_v)
|
, double extra_accel_v, double extra_decel_v)
|
||||||
{
|
{
|
||||||
struct move *m = move_alloc();
|
struct coord start_pos, axes_r;
|
||||||
|
start_pos.x = start_e_pos;
|
||||||
|
axes_r.x = 1.;
|
||||||
|
start_pos.y = start_pos.z = axes_r.y = axes_r.z = 0.;
|
||||||
|
|
||||||
// Setup velocity trapezoid
|
if (accel_t) {
|
||||||
m->print_time = print_time;
|
struct move *m = move_alloc();
|
||||||
m->move_t = accel_t + cruise_t + decel_t;
|
m->print_time = print_time;
|
||||||
m->accel_t = accel_t;
|
m->move_t = accel_t;
|
||||||
m->cruise_t = cruise_t;
|
m->start_v = start_v + extra_accel_v;
|
||||||
m->cruise_start_d = accel_t * (.5 * (cruise_v + start_v) + extra_accel_v);
|
m->half_accel = .5 * accel;
|
||||||
m->decel_start_d = m->cruise_start_d + cruise_t * cruise_v;
|
m->start_pos = start_pos;
|
||||||
|
m->axes_r = axes_r;
|
||||||
|
trapq_add_move(tq, m);
|
||||||
|
|
||||||
// Setup for accel/cruise/decel phases
|
print_time += accel_t;
|
||||||
m->cruise_v = cruise_v;
|
start_pos.x += move_get_distance(m, accel_t);
|
||||||
m->accel.c1 = start_v + extra_accel_v;
|
}
|
||||||
m->accel.c2 = .5 * accel;
|
if (cruise_t) {
|
||||||
m->decel.c1 = cruise_v + extra_decel_v;
|
struct move *m = move_alloc();
|
||||||
m->decel.c2 = -m->accel.c2;
|
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);
|
||||||
|
|
||||||
// Setup start distance
|
print_time += cruise_t;
|
||||||
m->start_pos.x = start_pos;
|
start_pos.x += move_get_distance(m, cruise_t);
|
||||||
m->axes_r.x = 1.;
|
}
|
||||||
|
if (decel_t) {
|
||||||
trapq_add_move(tq, m);
|
struct move *m = move_alloc();
|
||||||
|
m->print_time = print_time;
|
||||||
|
m->move_t = decel_t;
|
||||||
|
m->start_v = cruise_v + extra_decel_v;
|
||||||
|
m->half_accel = -.5 * accel;
|
||||||
|
m->start_pos = start_pos;
|
||||||
|
m->axes_r = axes_r;
|
||||||
|
trapq_add_move(tq, m);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,57 +28,59 @@ trapq_append(struct trapq *tq, double print_time
|
||||||
, double axes_d_x, double axes_d_y, double axes_d_z
|
, double axes_d_x, double axes_d_y, double axes_d_z
|
||||||
, double start_v, double cruise_v, double accel)
|
, double start_v, double cruise_v, double accel)
|
||||||
{
|
{
|
||||||
struct move *m = move_alloc();
|
struct coord axes_r, start_pos;
|
||||||
|
|
||||||
// Setup velocity trapezoid
|
|
||||||
m->print_time = print_time;
|
|
||||||
m->move_t = accel_t + cruise_t + decel_t;
|
|
||||||
m->accel_t = accel_t;
|
|
||||||
m->cruise_t = cruise_t;
|
|
||||||
m->cruise_start_d = accel_t * .5 * (cruise_v + start_v);
|
|
||||||
m->decel_start_d = m->cruise_start_d + cruise_t * cruise_v;
|
|
||||||
|
|
||||||
// Setup for accel/cruise/decel phases
|
|
||||||
m->cruise_v = cruise_v;
|
|
||||||
m->accel.c1 = start_v;
|
|
||||||
m->accel.c2 = .5 * accel;
|
|
||||||
m->decel.c1 = cruise_v;
|
|
||||||
m->decel.c2 = -m->accel.c2;
|
|
||||||
|
|
||||||
// Setup for move_get_coord()
|
|
||||||
m->start_pos.x = start_pos_x;
|
|
||||||
m->start_pos.y = start_pos_y;
|
|
||||||
m->start_pos.z = start_pos_z;
|
|
||||||
double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y
|
double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y
|
||||||
+ axes_d_z*axes_d_z);
|
+ axes_d_z*axes_d_z);
|
||||||
m->axes_r.x = axes_d_x * inv_move_d;
|
axes_r.x = axes_d_x * inv_move_d;
|
||||||
m->axes_r.y = axes_d_y * inv_move_d;
|
axes_r.y = axes_d_y * inv_move_d;
|
||||||
m->axes_r.z = axes_d_z * inv_move_d;
|
axes_r.z = axes_d_z * inv_move_d;
|
||||||
|
start_pos.x = start_pos_x;
|
||||||
|
start_pos.y = start_pos_y;
|
||||||
|
start_pos.z = start_pos_z;
|
||||||
|
|
||||||
trapq_add_move(tq, m);
|
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);
|
||||||
|
|
||||||
// Find the distance travel during acceleration/deceleration
|
print_time += accel_t;
|
||||||
static inline double
|
start_pos = move_get_coord(m, accel_t);
|
||||||
move_eval_accel(struct move_accel *ma, double move_time)
|
}
|
||||||
{
|
if (cruise_t) {
|
||||||
return (ma->c1 + ma->c2 * move_time) * move_time;
|
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
|
// Return the distance moved given a time in a move
|
||||||
inline double
|
inline double
|
||||||
move_get_distance(struct move *m, double move_time)
|
move_get_distance(struct move *m, double move_time)
|
||||||
{
|
{
|
||||||
if (unlikely(move_time < m->accel_t))
|
return (m->start_v + m->half_accel * move_time) * move_time;
|
||||||
// Acceleration phase of move
|
|
||||||
return move_eval_accel(&m->accel, move_time);
|
|
||||||
move_time -= m->accel_t;
|
|
||||||
if (likely(move_time <= m->cruise_t))
|
|
||||||
// Cruising phase
|
|
||||||
return m->cruise_start_d + m->cruise_v * move_time;
|
|
||||||
// Deceleration phase
|
|
||||||
move_time -= m->cruise_t;
|
|
||||||
return m->decel_start_d + move_eval_accel(&m->decel, move_time);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the XYZ coordinates given a time in a move
|
// Return the XYZ coordinates given a time in a move
|
||||||
|
|
|
@ -7,16 +7,9 @@ struct coord {
|
||||||
double x, y, z;
|
double x, y, z;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct move_accel {
|
|
||||||
double c1, c2;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct move {
|
struct move {
|
||||||
double print_time, move_t;
|
double print_time, move_t;
|
||||||
double accel_t, cruise_t;
|
double start_v, half_accel;
|
||||||
double cruise_start_d, decel_start_d;
|
|
||||||
double cruise_v;
|
|
||||||
struct move_accel accel, decel;
|
|
||||||
struct coord start_pos, axes_r;
|
struct coord start_pos, axes_r;
|
||||||
|
|
||||||
struct list_node node;
|
struct list_node node;
|
||||||
|
|
|
@ -306,7 +306,8 @@ class ToolHead:
|
||||||
move.start_v, move.cruise_v, move.accel)
|
move.start_v, move.cruise_v, move.accel)
|
||||||
if move.axes_d[3]:
|
if move.axes_d[3]:
|
||||||
self.extruder.move(next_move_time, move)
|
self.extruder.move(next_move_time, move)
|
||||||
next_move_time += move.accel_t + move.cruise_t + move.decel_t
|
next_move_time = (next_move_time + move.accel_t
|
||||||
|
+ move.cruise_t + move.decel_t)
|
||||||
# Generate steps for moves
|
# Generate steps for moves
|
||||||
if self.special_queuing_state == "Drip":
|
if self.special_queuing_state == "Drip":
|
||||||
self._update_drip_move_time(next_move_time)
|
self._update_drip_move_time(next_move_time)
|
||||||
|
|
Loading…
Reference in New Issue