diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index 54b07d59..00f835f5 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -33,10 +33,10 @@ class ForceMove: self.printer = config.get_printer() self.steppers = {} # Setup iterative solver + self.motion_queuing = self.printer.load_object(config, 'motion_queuing') + self.trapq = self.motion_queuing.allocate_trapq() + self.trapq_append = self.motion_queuing.lookup_trapq_append() ffi_main, ffi_lib = chelper.get_ffi() - self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_append = ffi_lib.trapq_append - self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves self.stepper_kinematics = ffi_main.gc( ffi_lib.cartesian_stepper_alloc(b'x'), ffi_lib.free) # Register commands @@ -90,8 +90,7 @@ class ForceMove: toolhead.flush_step_generation() stepper.set_trapq(prev_trapq) stepper.set_stepper_kinematics(prev_sk) - self.trapq_finalize_moves(self.trapq, print_time + 99999.9, - print_time + 99999.9) + self.motion_queuing.wipe_trapq(self.trapq) def _lookup_stepper(self, gcmd): name = gcmd.get('STEPPER') if name not in self.steppers: diff --git a/klippy/extras/manual_stepper.py b/klippy/extras/manual_stepper.py index 03816b5c..05899f58 100644 --- a/klippy/extras/manual_stepper.py +++ b/klippy/extras/manual_stepper.py @@ -25,10 +25,9 @@ class ManualStepper: self.pos_min = config.getfloat('position_min', None) self.pos_max = config.getfloat('position_max', None) # Setup iterative solver - ffi_main, ffi_lib = chelper.get_ffi() - self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_append = ffi_lib.trapq_append - self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves + self.motion_queuing = self.printer.load_object(config, 'motion_queuing') + self.trapq = self.motion_queuing.allocate_trapq() + self.trapq_append = self.motion_queuing.lookup_trapq_append() self.rail.setup_itersolve('cartesian_stepper_alloc', b'x') self.rail.set_trapq(self.trapq) # Registered with toolhead as an axtra axis @@ -76,8 +75,6 @@ class ManualStepper: self.sync_print_time() self.next_cmd_time = self._submit_move(self.next_cmd_time, movepos, speed, accel) - self.trapq_finalize_moves(self.trapq, self.next_cmd_time + 99999.9, - self.next_cmd_time + 99999.9) toolhead = self.printer.lookup_object('toolhead') toolhead.note_mcu_movequeue_activity(self.next_cmd_time) if sync: @@ -208,7 +205,7 @@ class ManualStepper: toolhead.drip_update_time(maxtime, drip_completion) # Clear trapq of any remaining parts of movement reactor = self.printer.get_reactor() - self.trapq_finalize_moves(self.trapq, reactor.NEVER, 0) + self.motion_queuing.wipe_trapq(self.trapq) self.rail.set_position([newpos[0], 0., 0.]) self.sync_print_time() def get_kinematics(self): diff --git a/klippy/extras/motion_queuing.py b/klippy/extras/motion_queuing.py index 4b5bd017..d8db880a 100644 --- a/klippy/extras/motion_queuing.py +++ b/klippy/extras/motion_queuing.py @@ -4,16 +4,36 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import logging +import chelper class PrinterMotionQueuing: def __init__(self, config): self.printer = config.get_printer() self.steppers = [] + self.trapqs = [] + ffi_main, ffi_lib = chelper.get_ffi() + self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves + def allocate_trapq(self): + ffi_main, ffi_lib = chelper.get_ffi() + trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) + self.trapqs.append(trapq) + return trapq def register_stepper(self, config, stepper): self.steppers.append(stepper) def flush_motion_queues(self, must_flush_time, max_step_gen_time): for stepper in self.steppers: stepper.generate_steps(max_step_gen_time) + def clean_motion_queues(self, trapq_free_time, clear_history_time): + for trapq in self.trapqs: + self.trapq_finalize_moves(trapq, trapq_free_time, + clear_history_time) + def wipe_trapq(self, trapq): + # Expire any remaining movement in the trapq (force to history list) + NEVER = 9999999999999999. + self.trapq_finalize_moves(trapq, NEVER, 0.) + def lookup_trapq_append(self): + ffi_main, ffi_lib = chelper.get_ffi() + return ffi_lib.trapq_append def load_config(config): return PrinterMotionQueuing(config) diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index e3e375bc..a89e3bdf 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -163,10 +163,9 @@ class PrinterExtruder: self.instant_corner_v = config.getfloat( 'instantaneous_corner_velocity', 1., minval=0.) # Setup extruder trapq (trapezoidal motion queue) - ffi_main, ffi_lib = chelper.get_ffi() - self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_append = ffi_lib.trapq_append - self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves + self.motion_queuing = self.printer.load_object(config, 'motion_queuing') + self.trapq = self.motion_queuing.allocate_trapq() + self.trapq_append = self.motion_queuing.lookup_trapq_append() # Setup extruder stepper self.extruder_stepper = None if (config.get('step_pin', None) is not None diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 3b9c0e8e..bc8c254d 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -245,14 +245,10 @@ class ToolHead: # Kinematic step generation scan window time tracking self.kin_flush_delay = SDS_CHECK_TIME self.kin_flush_times = [] - # Setup iterative solver - ffi_main, ffi_lib = chelper.get_ffi() - self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) - self.trapq_append = ffi_lib.trapq_append - self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves - # Motion flushing + # Setup for generating moves self.motion_queuing = self.printer.load_object(config, 'motion_queuing') - self.flush_trapqs = [self.trapq] + self.trapq = self.motion_queuing.allocate_trapq() + self.trapq_append = self.motion_queuing.lookup_trapq_append() # Create kinematics class gcode = self.printer.lookup_object('gcode') self.Coord = gcode.Coord @@ -287,8 +283,7 @@ class ToolHead: if not self.can_pause: clear_history_time = flush_time - MOVE_HISTORY_EXPIRE free_time = sg_flush_time - self.kin_flush_delay - for trapq in self.flush_trapqs: - self.trapq_finalize_moves(trapq, free_time, clear_history_time) + self.motion_queuing.clean_motion_queues(free_time, clear_history_time) # Flush stepcompress and mcu steppersync for m in self.all_mcus: m.flush_moves(flush_time, clear_history_time) @@ -484,32 +479,20 @@ class ToolHead: eventtime = self.reactor.pause(eventtime + 0.100) def set_extruder(self, extruder, extrude_pos): # XXX - should use add_extra_axis - prev_ea_trapq = self.extra_axes[0].get_trapq() - if prev_ea_trapq in self.flush_trapqs: - self.flush_trapqs.remove(prev_ea_trapq) self.extra_axes[0] = extruder self.commanded_pos[3] = extrude_pos - ea_trapq = extruder.get_trapq() - if ea_trapq is not None: - self.flush_trapqs.append(ea_trapq) def get_extruder(self): return self.extra_axes[0] def add_extra_axis(self, ea, axis_pos): self._flush_lookahead() self.extra_axes.append(ea) self.commanded_pos.append(axis_pos) - ea_trapq = ea.get_trapq() - if ea_trapq is not None: - self.flush_trapqs.append(ea_trapq) self.printer.send_event("toolhead:update_extra_axes") def remove_extra_axis(self, ea): self._flush_lookahead() if ea not in self.extra_axes: return ea_index = self.extra_axes.index(ea) + 3 - ea_trapq = ea.get_trapq() - if ea_trapq in self.flush_trapqs: - self.flush_trapqs.remove(ea_trapq) self.commanded_pos.pop(ea_index) self.extra_axes.pop(ea_index - 3) self.printer.send_event("toolhead:update_extra_axes") @@ -574,7 +557,7 @@ class ToolHead: next_move_time = self._drip_load_trapq(move) self.drip_update_time(next_move_time, drip_completion) # Move finished; cleanup any remnants on trapq - self.trapq_finalize_moves(self.trapq, self.reactor.NEVER, 0) + self.motion_queuing.wipe_trapq(self.trapq) # Misc commands def stats(self, eventtime): max_queue_time = max(self.print_time, self.last_flush_time)