diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 2c8f78f3..e26e31ba 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -58,15 +58,13 @@ defs_itersolve = """ """ defs_trapq = """ - struct move *move_alloc(void); - void move_fill(struct move *m, double print_time + 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_d_x, double axes_d_y, double axes_d_z , double start_v, double cruise_v, double accel); struct trapq *trapq_alloc(void); void trapq_free(struct trapq *tq); - void trapq_add_move(struct trapq *tq, struct move *m); void trapq_free_moves(struct trapq *tq, double print_time); """ @@ -94,7 +92,7 @@ defs_kin_winch = """ defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); - void extruder_move_fill(struct move *m, 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 start_v, double cruise_v, double accel , double extra_accel_v, double extra_decel_v); diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index eeb107ab..c86b35e3 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -215,7 +215,9 @@ itersolve_calc_position_from_coord(struct stepper_kinematics *sk { struct move m; memset(&m, 0, sizeof(m)); - move_fill(&m, 0., 0., 1., 0., x, y, z, 0., 1., 0., 0., 1., 0.); + m.start_pos.x = x; + m.start_pos.y = y; + m.start_pos.z = z; return sk->calc_position_cb(sk, &m, 0.); } diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 26513092..37c00c8b 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -30,12 +30,14 @@ extruder_stepper_alloc(void) // Populate a 'struct move' with an extruder velocity trapezoid void __visible -extruder_move_fill(struct move *m, double print_time - , double accel_t, double cruise_t, double decel_t - , double start_pos - , double start_v, double cruise_v, double accel - , double extra_accel_v, double extra_decel_v) +extruder_add_move(struct trapq *tq, double print_time + , double accel_t, double cruise_t, double decel_t + , double start_pos + , double start_v, double cruise_v, double accel + , double extra_accel_v, double extra_decel_v) { + struct move *m = move_alloc(); + // Setup velocity trapezoid m->print_time = print_time; m->move_t = accel_t + cruise_t + decel_t; @@ -54,4 +56,6 @@ extruder_move_fill(struct move *m, double print_time // Setup start distance m->start_pos.x = start_pos; m->axes_r.x = 1.; + + trapq_add_move(tq, m); } diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index 828d48cf..966c8a45 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -12,7 +12,7 @@ #include "trapq.h" // move_get_coord // Allocate a new 'move' object -struct move * __visible +struct move * move_alloc(void) { struct move *m = malloc(sizeof(*m)); @@ -20,14 +20,16 @@ move_alloc(void) return m; } -// Populate a 'struct move' with a velocity trapezoid +// Fill and add a move to the trapezoid velocity queue void __visible -move_fill(struct move *m, 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_d_x, double axes_d_y, double axes_d_z - , double start_v, double cruise_v, double accel) +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_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; @@ -52,6 +54,8 @@ move_fill(struct move *m, double print_time 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; + + trapq_add_move(tq, m); } // Find the distance travel during acceleration/deceleration @@ -111,12 +115,10 @@ trapq_free(struct trapq *tq) } // Add a move to the trapezoid velocity queue -void __visible +void trapq_add_move(struct trapq *tq, struct move *m) { - struct move *nm = move_alloc(); - memcpy(nm, m, sizeof(*nm)); - list_add_tail(&nm->node, &tq->moves); + list_add_tail(&m->node, &tq->moves); } // Free any moves older than `print_time` from the trapezoid velocity queue diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index e568a1ab..0c81b330 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -22,19 +22,18 @@ struct move { struct list_node node; }; -struct move *move_alloc(void); -void move_fill(struct move *m, 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_d_x, double axes_d_y, double axes_d_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 { struct list_head moves; }; +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_d_x, double axes_d_y, double axes_d_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_add_move(struct trapq *tq, struct move *m); diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index 63e1c293..fe3b3a60 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -28,10 +28,8 @@ class ForceMove: self.steppers = {} # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() - self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) - self.move_fill = ffi_lib.move_fill self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_add_move = ffi_lib.trapq_add_move + self.trapq_append = ffi_lib.trapq_append self.trapq_free_moves = ffi_lib.trapq_free_moves self.stepper_kinematics = ffi_main.gc( ffi_lib.cartesian_stepper_alloc('x'), ffi_lib.free) @@ -70,9 +68,8 @@ class ForceMove: prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics) stepper.set_position((0., 0., 0.)) accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel) - self.move_fill(self.cmove, print_time, accel_t, cruise_t, accel_t, - 0., 0., 0., dist, 0., 0., 0., cruise_v, accel) - self.trapq_add_move(self.trapq, self.cmove) + self.trapq_append(self.trapq, print_time, accel_t, cruise_t, accel_t, + 0., 0., 0., dist, 0., 0., 0., cruise_v, accel) print_time += accel_t + cruise_t + accel_t stepper.generate_steps(print_time) self.trapq_free_moves(self.trapq, print_time) diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py index a8448e72..9b985007 100644 --- a/klippy/extras/manual_stepper.py +++ b/klippy/extras/manual_stepper.py @@ -23,10 +23,8 @@ class ManualStepper: self.next_cmd_time = 0. # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() - self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) - self.move_fill = ffi_lib.move_fill self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_add_move = ffi_lib.trapq_add_move + self.trapq_append = ffi_lib.trapq_append self.trapq_free_moves = ffi_lib.trapq_free_moves self.stepper.setup_itersolve('cartesian_stepper_alloc', 'x') self.stepper.set_trapq(self.trapq) @@ -58,11 +56,10 @@ class ManualStepper: dist = movepos - cp accel_t, cruise_t, cruise_v = force_move.calc_move_time( dist, speed, accel) - self.move_fill(self.cmove, self.next_cmd_time, - accel_t, cruise_t, accel_t, - cp, 0., 0., dist, 0., 0., - 0., cruise_v, accel) - self.trapq_add_move(self.trapq, self.cmove) + self.trapq_append(self.trapq, self.next_cmd_time, + accel_t, cruise_t, accel_t, + cp, 0., 0., dist, 0., 0., + 0., cruise_v, accel) self.next_cmd_time += accel_t + cruise_t + accel_t self.stepper.generate_steps(self.next_cmd_time) self.trapq_free_moves(self.trapq, self.next_cmd_time) diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index a9215688..6b9245d4 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -53,10 +53,8 @@ class PrinterExtruder: self.extrude_pos = 0. # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() - self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) - self.extruder_move_fill = ffi_lib.extruder_move_fill + self.extruder_add_move = ffi_lib.extruder_add_move self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_add_move = ffi_lib.trapq_add_move self.trapq_free_moves = ffi_lib.trapq_free_moves self.stepper.setup_itersolve('extruder_stepper_alloc') self.stepper.set_trapq(self.trapq) @@ -205,10 +203,9 @@ class PrinterExtruder: extra_decel_v = extra_decel_d / decel_t # Generate steps - self.extruder_move_fill( - self.cmove, print_time, accel_t, cruise_t, decel_t, start_pos, + self.extruder_add_move( + self.trapq, print_time, accel_t, cruise_t, decel_t, start_pos, start_v, cruise_v, accel, extra_accel_v, extra_decel_v) - self.trapq_add_move(self.trapq, self.cmove) self.extrude_pos = start_pos + axis_d cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" def cmd_default_SET_PRESSURE_ADVANCE(self, params): diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 5c93dfa6..0c4dd1bb 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -18,7 +18,6 @@ class Move: self.end_pos = tuple(end_pos) self.accel = toolhead.max_accel velocity = min(speed, toolhead.max_velocity) - self.cmove = toolhead.cmove self.is_kinematic_move = True self.axes_d = axes_d = [end_pos[i] - start_pos[i] for i in (0, 1, 2, 3)] self.move_d = move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) @@ -97,13 +96,12 @@ class Move: # Generate step times for the move next_move_time = self.toolhead.get_next_move_time() if self.is_kinematic_move: - self.toolhead.move_fill( - self.cmove, next_move_time, + self.toolhead.trapq_append( + self.toolhead.trapq, next_move_time, self.accel_t, self.cruise_t, self.decel_t, self.start_pos[0], self.start_pos[1], self.start_pos[2], self.axes_d[0], self.axes_d[1], self.axes_d[2], self.start_v, self.cruise_v, self.accel) - self.toolhead.trapq_add_move(self.toolhead.trapq, self.cmove) if self.axes_d[3]: self.toolhead.extruder.move(next_move_time, self) self.toolhead.update_move_time( @@ -247,10 +245,8 @@ class ToolHead: self.drip_completion = None # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() - self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) - self.move_fill = ffi_lib.move_fill self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_add_move = ffi_lib.trapq_add_move + self.trapq_append = ffi_lib.trapq_append self.trapq_free_moves = ffi_lib.trapq_free_moves self.move_handlers = [] # Create kinematics class