From 076a66f791414b0eaf9a9a9c8907b20be327385a Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 5 Nov 2019 15:42:19 -0500 Subject: [PATCH] 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 --- klippy/chelper/__init__.py | 2 +- klippy/chelper/kin_extruder.c | 59 +++++++++++++++--------- klippy/chelper/trapq.c | 84 ++++++++++++++++++----------------- klippy/chelper/trapq.h | 9 +--- klippy/toolhead.py | 3 +- 5 files changed, 86 insertions(+), 71 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index e26e31ba..afd4d922 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -93,7 +93,7 @@ defs_kin_winch = """ defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); 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 extra_accel_v, double extra_decel_v); """ diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 37c00c8b..e5e440f4 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -32,30 +32,49 @@ extruder_stepper_alloc(void) void __visible extruder_add_move(struct trapq *tq, double print_time , 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 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 - 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) + extra_accel_v); - m->decel_start_d = m->cruise_start_d + cruise_t * cruise_v; + if (accel_t) { + struct move *m = move_alloc(); + m->print_time = print_time; + m->move_t = accel_t; + m->start_v = start_v + extra_accel_v; + m->half_accel = .5 * accel; + m->start_pos = start_pos; + m->axes_r = axes_r; + trapq_add_move(tq, m); - // Setup for accel/cruise/decel phases - m->cruise_v = cruise_v; - m->accel.c1 = start_v + extra_accel_v; - m->accel.c2 = .5 * accel; - m->decel.c1 = cruise_v + extra_decel_v; - m->decel.c2 = -m->accel.c2; + print_time += accel_t; + start_pos.x += move_get_distance(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); - // Setup start distance - m->start_pos.x = start_pos; - m->axes_r.x = 1.; - - trapq_add_move(tq, m); + print_time += cruise_t; + start_pos.x += move_get_distance(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 + extra_decel_v; + m->half_accel = -.5 * accel; + m->start_pos = start_pos; + m->axes_r = axes_r; + trapq_add_move(tq, m); + } } diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index 966c8a45..851a15b7 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -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 start_v, double cruise_v, double accel) { - struct move *m = move_alloc(); - - // 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; + struct coord axes_r, start_pos; double inv_move_d = 1. / sqrt(axes_d_x*axes_d_x + axes_d_y*axes_d_y + axes_d_z*axes_d_z); - m->axes_r.x = axes_d_x * inv_move_d; - m->axes_r.y = axes_d_y * inv_move_d; - m->axes_r.z = axes_d_z * inv_move_d; + axes_r.x = axes_d_x * inv_move_d; + axes_r.y = axes_d_y * 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 -static inline double -move_eval_accel(struct move_accel *ma, double move_time) -{ - return (ma->c1 + ma->c2 * move_time) * move_time; + 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) { - if (unlikely(move_time < m->accel_t)) - // 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 (m->start_v + m->half_accel * move_time) * move_time; } // Return the XYZ coordinates given a time in a move diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index 0c81b330..787d83a4 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -7,16 +7,9 @@ struct coord { double x, y, z; }; -struct move_accel { - double c1, c2; -}; - struct move { double print_time, move_t; - double accel_t, cruise_t; - double cruise_start_d, decel_start_d; - double cruise_v; - struct move_accel accel, decel; + double start_v, half_accel; struct coord start_pos, axes_r; struct list_node node; diff --git a/klippy/toolhead.py b/klippy/toolhead.py index e1f2ddc9..f23f2bc3 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -306,7 +306,8 @@ class ToolHead: move.start_v, move.cruise_v, move.accel) if move.axes_d[3]: 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 if self.special_queuing_state == "Drip": self._update_drip_move_time(next_move_time)