def __init_path_planner(self): self.native_planner = PathPlannerNative() self.native_planner.initPRU(self.pru_firmware.get_firmware(0), self.pru_firmware.get_firmware(1)) self.native_planner.setPrintAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ / 1000.0) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) self.native_planner.setExtruder(0) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative() self.native_planner.initPRU(self.pru_firmware.get_firmware(0), self.pru_firmware.get_firmware(1)) self.native_planner.setPrintAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ /1000.0) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) self.native_planner.setExtruder(0) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative( int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAxisStepsPerMeter( tuple(self.printer.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(self.printer.max_speeds)) self.native_planner.setMinSpeeds(tuple(self.printer.min_speeds)) self.native_planner.setAcceleration(tuple(self.printer.acceleration)) self.native_planner.setJerks(tuple(self.printer.jerks)) self.native_planner.setPrintMoveBufferWait( int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime( int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime( int(self.printer.max_buffered_move_time)) self.native_planner.setSoftEndstopsMin(tuple(self.printer.soft_min)) self.native_planner.setSoftEndstopsMax(tuple(self.printer.soft_max)) self.native_planner.setBedCompensationMatrix( tuple(np.identity(3).ravel())) self.native_planner.setMaxPathLength(self.printer.max_length) self.native_planner.setAxisConfig(self.printer.axis_config) self.native_planner.delta_bot.setMainDimensions( Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset( Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError(Delta.A_radial, Delta.B_radial, Delta.C_radial) self.native_planner.delta_bot.setTangentError(Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() self.configure_slaves() self.native_planner.setBacklashCompensation( tuple(self.printer.backlash_compensation)) self.native_planner.setState(self.prev.end_pos) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative( int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setPrintAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ / 1000.0) self.native_planner.setPrintMoveBufferWait( int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime( int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime( int(self.printer.max_buffered_move_time)) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) e.setDirectionInverted(False) self.native_planner.setExtruder(0) self.native_planner.setDriveSystem(Path.axis_config) logging.info("Setting drive system to " + str(Path.axis_config)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAxisStepsPerMeter(tuple(self.printer.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(self.printer.max_speeds)) self.native_planner.setMinSpeeds(tuple(self.printer.min_speeds)) self.native_planner.setAcceleration(tuple(self.printer.acceleration)) self.native_planner.setJerks(tuple(self.printer.jerks)) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) self.native_planner.setSoftEndstopsMin(tuple(self.printer.soft_min)) self.native_planner.setSoftEndstopsMax(tuple(self.printer.soft_max)) self.native_planner.setBedCompensationMatrix(tuple(self.printer.matrix_bed_comp.ravel())) self.native_planner.setMaxPathLength(self.printer.max_length) self.native_planner.setAxisConfig(self.printer.axis_config) self.native_planner.delta_bot.setMainDimensions(Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset(Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError(Delta.A_radial, Delta.B_radial, Delta.C_radial); self.native_planner.delta_bot.setTangentError(Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() self.native_planner.enableSlaves(self.printer.has_slaves) if self.printer.has_slaves: for master in Printer.AXES: slave = self.printer.slaves[master] if slave: master_index = Printer.axis_to_index(master) slave_index = Printer.axis_to_index(slave) self.native_planner.addSlave(int(master_index), int(slave_index)) logging.debug("Axis " + str(slave_index) + " is slaved to axis " + str(master_index)) self.native_planner.setBacklashCompensation(tuple(self.printer.backlash_compensation)); self.native_planner.setState(self.prev.end_pos) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAcceleration(tuple(Path.acceleration)) self.native_planner.setAxisStepsPerMeter(tuple(Path.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(Path.max_speeds)) self.native_planner.setMinSpeeds(tuple(Path.min_speeds)) self.native_planner.setJerks(tuple(Path.jerks)) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread()
def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setPrintAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter(tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates(tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ /1000.0) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) e.setDirectionInverted(False) self.native_planner.setExtruder(0) self.native_planner.setDriveSystem(Path.axis_config) logging.info("Setting drive system to "+str(Path.axis_config)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread()
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.printer.path_planner = self self.travel_length = { "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0, "A": 0.0, "B": 0.0, "C": 0.0 } self.center_offset = { "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0, "A": 0.0, "B": 0.0, "C": 0.0 } self.home_pos = { "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0, "A": 0.0, "B": 0.0, "C": 0.0 } self.prev = G92Path( { "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0, "A": 0.0, "B": 0.0, "C": 0.0 }, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None # do some checking of values self.printer.check_values() def __init_path_planner(self): self.native_planner = PathPlannerNative( int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAxisStepsPerMeter( tuple(self.printer.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(self.printer.max_speeds)) self.native_planner.setMinSpeeds(tuple(self.printer.min_speeds)) self.native_planner.setAcceleration(tuple(self.printer.acceleration)) self.native_planner.setJerks(tuple(self.printer.jerks)) self.native_planner.setPrintMoveBufferWait( int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime( int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime( int(self.printer.max_buffered_move_time)) self.native_planner.setSoftEndstopsMin(tuple(self.printer.soft_min)) self.native_planner.setSoftEndstopsMax(tuple(self.printer.soft_max)) self.native_planner.setBedCompensationMatrix( tuple(np.identity(3).ravel())) self.native_planner.setMaxPathLength(self.printer.max_length) self.native_planner.setAxisConfig(self.printer.axis_config) self.native_planner.delta_bot.setMainDimensions( Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset( Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError(Delta.A_radial, Delta.B_radial, Delta.C_radial) self.native_planner.delta_bot.setTangentError(Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() self.configure_slaves() self.native_planner.setBacklashCompensation( tuple(self.printer.backlash_compensation)) self.native_planner.setState(self.prev.end_pos) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread() def configure_slaves(self): self.native_planner.enableSlaves(self.printer.has_slaves) if self.printer.has_slaves: for master in Printer.AXES: slave = self.printer.slaves[master] if slave: master_index = Printer.axis_to_index(master) slave_index = Printer.axis_to_index(slave) self.native_planner.addSlave(int(master_index), int(slave_index)) logging.debug("Axis " + str(slave_index) + " is slaved to axis " + str(master_index)) def restart(self): self.native_planner.stopThread(True) self.__init_path_planner() def update_steps_pr_meter(self): """ Update steps pr meter from the path """ self.native_planner.setAxisStepsPerMeter( tuple(self.printer.steps_pr_meter)) def update_backlash(self): """ Update steps pr meter from the path """ self.native_planner.setBacklashCompensation( tuple(self.printer.backlash_compensation)) def get_current_pos(self, mm=False, ideal=False): """ Get the current pos as a dict """ if mm: scale = 1000.0 else: scale = 1.0 state = self.native_planner.getState() if ideal: state = self.prev.ideal_end_pos pos = {} for index, axis in enumerate(Printer.AXES[:Printer.MAX_AXES]): pos[axis] = state[index] * scale return pos def get_extruder_pos(self, ext_nr): """ Return the current position of this extruder """ state = self.native_planner.getState() return state[3 + ext_nr] def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def wait_until_sync_event(self): """ Blocks until a PRU sync event occurs """ return (self.native_planner.waitUntilSyncEvent() > 0) def clear_sync_event(self): """ Resumes/Clears a pending sync event """ self.native_planner.clearSyncEvent() def queue_sync_event(self, isBlocking): """ Returns True if a sync event has been queued. False on failure.(use wait_until_done() instead) """ return self.native_planner.queueSyncEvent(isBlocking) def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): ''' Temporary pause of planner ''' self.native_planner.suspend() def resume(self): ''' resume a paused planner ''' self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing internal " + str(axis)) path_search = {} path_backoff = {} path_fine_search = {} path_center = {} path_zero = {} speed = self.printer.home_speed[0] # TODO: speed for each axis accel = self.printer.acceleration[0] # TODO: accel for each axis for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue logging.debug("Doing homing for " + str(a)) if self.printer.home_speed[Printer.axis_to_index(a)] < 0: # Search to positive ends path_search[a] = self.travel_length[a] path_center[a] = self.center_offset[a] else: # Search to negative ends path_search[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] backoff_length = -np.sign( path_search[a]) * self.printer.home_backoff_offset[ Printer.axis_to_index(a)] path_backoff[a] = backoff_length path_fine_search[a] = -backoff_length * 1.2 speed = min(abs(speed), abs(self.printer.home_speed[Printer.axis_to_index(a)])) accel = min(accel, self.printer.acceleration[Printer.axis_to_index(a)]) fine_search_speed = min( abs(speed), abs(self.printer.home_backoff_speed[Printer.axis_to_index(a)])) logging.debug("Search: %s at %s m/s, %s m/s^2" % (path_search, speed, accel)) logging.debug("Backoff to: %s" % path_backoff) logging.debug("Fine search: %s" % path_fine_search) logging.debug("Center: %s" % path_center) # Move until endstop is hit p = RelativePath(path_search, speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() logging.debug("Coarse search done!") # Reset position to offset p = G92Path(path_center) self.add_path(p) self.wait_until_done() # Back off a bit p = RelativePath(path_backoff, speed, accel, True, False, True, False) self.add_path(p) # Hit the endstop slowly p = RelativePath(path_fine_search, fine_search_speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset (final) position to offset p = G92Path(path_center) self.add_path(p) return path_center, speed def _go_to_home(self, axis): """ go to the designated home position do this as a separate call from _home_internal due to delta platforms performing home in cartesian mode """ path_home = {} speed = self.printer.home_speed[0] accel = self.printer.acceleration[0] for a in axis: path_home[a] = self.home_pos[a] speed = min(abs(speed), abs(self.printer.home_speed[Printer.axis_to_index(a)])) logging.debug("Home: %s" % path_home) # Move to home position p = AbsolutePath(path_home, speed, accel, True, False, False, False) self.add_path(p) self.wait_until_done() # Due to rounding errors, we explicitly set the found # position to the right value. # Reset (final) position to offset p = G92Path(path_home) self.add_path(p) return def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # allow for endstops that may only be active during homing self.printer.homing(True) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if self.printer.axis_config == Printer.AXIS_CONFIG_CORE_XY or \ self.printer.axis_config == Printer.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif self.printer.axis_config == Printer.AXIS_CONFIG_DELTA: if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({"X", "Y", "Z" })) # Deltas must home all axes. self.printer.axis_config = Printer.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) self.printer.axis_config = Printer.AXIS_CONFIG_DELTA # homing was performed in cartesian mode # need to convert back to delta Az = path_center['X'] Bz = path_center['Y'] Cz = path_center['Z'] z_offset = self.native_planner.delta_bot.vertical_offset( Az, Bz, Cz) # vertical offset xyz = self.native_planner.delta_bot.forward_kinematics( Az, Bz, Cz) # effector position xyz[2] += z_offset path = {'X': xyz[0], 'Y': xyz[1], 'Z': xyz[2]} logging.debug("Delta Home: " + str(xyz)) p = G92Path(path) self.add_path(p) self.wait_until_done() else: # AXIS_CONFIG_XY self._home_internal(axis) # allow for endstops that may only be active during homing self.printer.homing(False) # go to the designated home position self._go_to_home(axis) # Reset backlash compensation self.native_planner.resetBacklash() logging.debug("homing done for " + str(axis)) return def probe(self, z, speed, accel): self.wait_until_done() self.printer.ensure_steppers_enabled() # save the starting position start_pos = self.get_current_pos(ideal=True) start_state = self.native_planner.getState() # calculate how many steps the requested z movement will require steps = np.ceil(z * self.printer.steps_pr_meter[2]) z_dist = steps / self.printer.steps_pr_meter[2] logging.debug("Steps total: " + str(steps)) # select the relative end point # this is not axis_config dependent as we are not swapping # axis_config like we do when homing end = {"Z": -z_dist} # tell the printer we are now in homing mode (updates firmware if required) self.printer.homing(True) # add a relative move to the path planner # this tells the head to move down a set distance # the probe end-stop should be triggered during this move path = RelativePath(end, speed, accel, cancelable=True, use_bed_matrix=True, use_backlash_compensation=True, enable_soft_endstops=False) self.add_path(path) self.wait_until_done() # get the number of steps that we haven't done steps_remaining = PruInterface.get_steps_remaining() logging.debug("Steps remaining : " + str(steps_remaining)) # Calculate how many steps the Z axis moved steps -= steps_remaining z_dist = steps / self.printer.steps_pr_meter[2] # make a move to take us back to where we started end = {"Z": z_dist} path = RelativePath(end, speed, accel, cancelable=True, use_bed_matrix=True, use_backlash_compensation=True, enable_soft_endstops=False) self.add_path(path) self.wait_until_done() # reset position back to where we actually are path = G92Path({"Z": start_pos["Z"]}, use_bed_matrix=True) self.add_path(path) self.wait_until_done() start_state = self.native_planner.getState() # tell the printer we are no longer in homing mode (updates firmware if required) self.printer.homing(False) return -z_dist + start_pos["Z"] def autocalibrate_delta_printer(self, num_factors, simulate_only, probe_points, print_head_zs): if self.printer.axis_config != Printer.AXIS_CONFIG_DELTA: logging.warning("autocalibrate_delta_printer only supports " "delta printers") return params = delta_auto_calibration(Delta, self.center_offset, num_factors, simulate_only, probe_points, print_head_zs) # update the native planner with the new values self.native_planner.delta_bot.setMainDimensions( Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset( Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError(Delta.A_radial, Delta.B_radial, Delta.C_radial) self.native_planner.delta_bot.setTangentError(Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() return params def add_path(self, new): """ Add a path segment to the path planner """ """ This code, and the native planner, needs to be updated for reach. """ # Link to the previous segment in the chain new.set_prev(self.prev) # NOTE: printing the added path slows things down SIGNIFICANTLY #logging.debug("path added: "+ str(new)) if new.is_G92(): self.native_planner.setState(tuple(new.end_pos)) elif new.needs_splitting(): #TODO: move this to C++ # this branch splits up any G2 or G3 movements (arcs) # should be moved to C++ as it is math heavy # need to convert it to linear segments before feeding to the queue # as we want to keep the queue only dealing with linear stuff for simplicity for seg in new.get_segments(): self.add_path(seg) else: self.printer.ensure_steppers_enabled() optimize = new.movement != Path.RELATIVE tool_axis = Printer.axis_to_index(self.printer.current_tool) self.native_planner.setAxisConfig(int(self.printer.axis_config)) # Start_pos is unused. TODO: Remove it. # Bed matrix behaviour is handled in Python space, it is fast enough for that. self.native_planner.queueMove( (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), #tuple(new.start_pos), tuple(new.end_pos), new.speed, new.accel, bool(new.cancelable), bool(optimize), bool(new.enable_soft_endstops), False, #bool(new.use_bed_matrix), bool(new.use_backlash_compensation), int(tool_axis), True) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. # make sure that the current state of the printer is correct self.prev.end_pos = self.native_planner.getState() #logging.debug("end pos: "+ str(self.prev.end_pos)) def set_extruder(self, ext_nr): """ TODO: does this function do anything? Should it be setting the tool axis? """ if ext_nr in range(Printer.MAX_AXES - 3): logging.debug("Selecting " + str(ext_nr))
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.printer.path_planner = self self.travel_length = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.center_offset = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.home_pos = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.prev = G92Path({"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0}, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None # do some checking of values self.printer.check_values() def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAxisStepsPerMeter(tuple(self.printer.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(self.printer.max_speeds)) self.native_planner.setMinSpeeds(tuple(self.printer.min_speeds)) self.native_planner.setAcceleration(tuple(self.printer.acceleration)) self.native_planner.setJerks(tuple(self.printer.jerks)) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) self.native_planner.setSoftEndstopsMin(tuple(self.printer.soft_min)) self.native_planner.setSoftEndstopsMax(tuple(self.printer.soft_max)) self.native_planner.setBedCompensationMatrix(tuple(self.printer.matrix_bed_comp.ravel())) self.native_planner.setMaxPathLength(self.printer.max_length) self.native_planner.setAxisConfig(self.printer.axis_config) self.native_planner.delta_bot.setMainDimensions(Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset(Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError(Delta.A_radial, Delta.B_radial, Delta.C_radial); self.native_planner.delta_bot.setTangentError(Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() self.native_planner.enableSlaves(self.printer.has_slaves) if self.printer.has_slaves: for master in Printer.AXES: slave = self.printer.slaves[master] if slave: master_index = Printer.axis_to_index(master) slave_index = Printer.axis_to_index(slave) self.native_planner.addSlave(int(master_index), int(slave_index)) logging.debug("Axis " + str(slave_index) + " is slaved to axis " + str(master_index)) self.native_planner.setBacklashCompensation(tuple(self.printer.backlash_compensation)); self.native_planner.setState(self.prev.end_pos) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread() def restart(self): self.native_planner.stopThread(True) self.__init_path_planner() def update_steps_pr_meter(self): """ Update steps pr meter from the path """ self.native_planner.setAxisStepsPerMeter(tuple(self.printer.steps_pr_meter)) def update_backlash(self): """ Update steps pr meter from the path """ self.native_planner.setBacklashCompensation(tuple(self.printer.backlash_compensation)); def get_current_pos(self, mm=False): """ Get the current pos as a dict """ if mm: scale = 1000.0 else: scale = 1.0 state = self.native_planner.getState() pos = {} for index, axis in enumerate(Printer.AXES[:Printer.MAX_AXES]): pos[axis] = state[index]*scale return pos def get_extruder_pos(self, ext_nr): """ Return the current position of this extruder """ state = self.native_planner.getState() return state[3+ext_nr] def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def wait_until_sync_event(self): """ Blocks until a PRU sync event occurs """ return (self.native_planner.waitUntilSyncEvent() > 0) def clear_sync_event(self): """ Resumes/Clears a pending sync event """ self.native_planner.clearSyncEvent() def queue_sync_event(self, isBlocking): """ Returns True if a sync event has been queued. False on failure.(use wait_until_done() instead) """ return self.native_planner.queueSyncEvent(isBlocking) def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): ''' Temporary pause of planner ''' self.native_planner.suspend() def resume(self): ''' resume a paused planner ''' self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing internal " + str(axis)) path_search = {} path_backoff = {} path_fine_search = {} path_center = {} path_zero = {} speed = self.printer.home_speed[0] # TODO: speed for each axis accel = self.printer.acceleration[0] # TODO: accel for each axis for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue logging.debug("Doing homing for " + str(a)) if self.printer.home_speed[Printer.axis_to_index(a)] < 0: # Search to positive ends path_search[a] = self.travel_length[a] path_center[a] = self.center_offset[a] else: # Search to negative ends path_search[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] backoff_length = -np.sign(path_search[a]) * self.printer.home_backoff_offset[Printer.axis_to_index(a)] path_backoff[a] = backoff_length; path_fine_search[a] = -backoff_length * 1.2; speed = min(abs(speed), abs(self.printer.home_speed[Printer.axis_to_index(a)])) accel = min(accel, self.printer.acceleration[Printer.axis_to_index(a)]) fine_search_speed = min(abs(speed), abs(self.printer.home_backoff_speed[Printer.axis_to_index(a)])) logging.debug("Search: %s at %s m/s, %s m/s^2" % (path_search, speed, accel)) logging.debug("Backoff to: %s" % path_backoff) logging.debug("Fine search: %s" % path_fine_search) logging.debug("Center: %s" % path_center) # Move until endstop is hit p = RelativePath(path_search, speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() logging.debug("Coarse search done!") # Reset position to offset p = G92Path(path_center) self.add_path(p) self.wait_until_done() # Back off a bit p = RelativePath(path_backoff, speed, accel, True, False, True, False) self.add_path(p) # Hit the endstop slowly p = RelativePath(path_fine_search, fine_search_speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset (final) position to offset p = G92Path(path_center) self.add_path(p) return path_center, speed def _go_to_home(self, axis): """ go to the designated home position do this as a separate call from _home_internal due to delta platforms performing home in cartesian mode """ path_home = {} speed = self.printer.home_speed[0] accel = self.printer.acceleration[0] for a in axis: path_home[a] = self.home_pos[a] speed = min(abs(speed), abs(self.printer.home_speed[Printer.axis_to_index(a)])) logging.debug("Home: %s" % path_home) # Move to home position p = AbsolutePath(path_home, speed, accel, True, False, False, False) self.add_path(p) self.wait_until_done() # Due to rounding errors, we explicitly set the found # position to the right value. # Reset (final) position to offset p = G92Path(path_home) self.add_path(p) return def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # allow for endstops that may only be active during homing self.printer.homing(True) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if self.printer.axis_config == Printer.AXIS_CONFIG_CORE_XY or \ self.printer.axis_config == Printer.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif self.printer.axis_config == Printer.AXIS_CONFIG_DELTA: if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({"X", "Y", "Z"})) # Deltas must home all axes. self.printer.axis_config = Printer.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) self.printer.axis_config = Printer.AXIS_CONFIG_DELTA # homing was performed in cartesian mode # need to convert back to delta Az = path_center['X'] Bz = path_center['Y'] Cz = path_center['Z'] z_offset = self.native_planner.delta_bot.vertical_offset(Az,Bz,Cz) # vertical offset xyz = self.native_planner.delta_bot.forward_kinematics(Az, Bz, Cz) # effector position xyz[2] += z_offset path = {'X':xyz[0], 'Y':xyz[1], 'Z':xyz[2]} logging.debug("Delta Home: " + str(xyz)) p = G92Path(path) self.add_path(p) self.wait_until_done() else: # AXIS_CONFIG_XY self._home_internal(axis) # allow for endstops that may only be active during homing self.printer.homing(False) # go to the designated home position self._go_to_home(axis) # Reset backlash compensation self.native_planner.resetBacklash() logging.debug("homing done for " + str(axis)) return def probe(self, z, speed, accel): self.wait_until_done() self.printer.ensure_steppers_enabled() # save the starting position start = self.get_current_pos() # calculate how many steps the requested z movement will require steps = np.ceil(z*self.printer.steps_pr_meter[2]) z_dist = steps/self.printer.steps_pr_meter[2] logging.debug("Steps total: "+str(steps)) # select the relative end point # this is not axis_config dependent as we are not swapping # axis_config like we do when homing end = {"Z":-z_dist} # tell the printer we are now in homing mode (updates firmware if required) self.printer.homing(True) # add a relative move to the path planner # this tells the head to move down a set distance # the probe end-stop should be triggered during this move path = RelativePath(end, speed, accel, cancelable=True, use_bed_matrix=False, use_backlash_compensation=True, enable_soft_endstops=False) self.add_path(path) self.wait_until_done() # get the number of steps that we haven't done steps_remaining = PruInterface.get_steps_remaining() logging.debug("Steps remaining : "+str(steps_remaining)) # Calculate how many steps the Z axis moved steps -= steps_remaining z_dist = steps/self.printer.steps_pr_meter[2] # make a move to take us back to where we started end = {"Z":z_dist} path = RelativePath(end, speed, accel, cancelable=True, use_bed_matrix=False, use_backlash_compensation=True, enable_soft_endstops=False) self.add_path(path) self.wait_until_done() # reset position back to where we actually are path = G92Path(start) self.add_path(path) self.wait_until_done() # tell the printer we are no longer in homing mode (updates firmware if required) self.printer.homing(False) return -z_dist def update_autolevel_matrix(self, probe_points, probe_heights): """ updates the bed compensation matrix """ logging.debug("updating bed compensation matrix") logging.debug("Probe points = " + str(probe_points)) logging.debug("Probe heights = " + str(probe_heights)) self.printer.matrix_bed_comp = BedCompensation.create_rotation_matrix(probe_points, probe_heights) self.native_planner.setBedCompensationMatrix(tuple(self.printer.matrix_bed_comp.ravel())) return def autocalibrate_delta_printer(self, num_factors, max_std, simulate_only, probe_points, print_head_zs): if self.printer.axis_config != Printer.AXIS_CONFIG_DELTA: logging.warning("autocalibrate_delta_printer only supports " "delta printers") return delta_auto_calibration(Delta, self.center_offset, num_factors, max_std, simulate_only, probe_points, print_head_zs) # update the native planner with the new values self.native_planner.delta_bot.setMainDimensions( Delta.Hez, Delta.L, Delta.r) self.native_planner.delta_bot.setEffectorOffset( Delta.Ae, Delta.Be, Delta.Ce) self.native_planner.delta_bot.setRadialError( Delta.A_radial, Delta.B_radial, Delta.C_radial) self.native_planner.delta_bot.setTangentError( Delta.A_tangential, Delta.B_tangential, Delta.C_tangential) self.native_planner.delta_bot.recalculate() def add_path(self, new): """ Add a path segment to the path planner """ """ This code, and the native planner, needs to be updated for reach. """ # Link to the previous segment in the chain new.set_prev(self.prev) # NOTE: printing the added path slows things down SIGNIFICANTLY #logging.debug("path added: "+ str(new)) if new.is_G92(): self.native_planner.setState(tuple(new.end_pos)) elif new.needs_splitting(): #TODO: move this to C++ # this branch splits up any G2 or G3 movements (arcs) # should be moved to C++ as it is math heavy # need to convert it to linear segments before feeding to the queue # as we want to keep the queue only dealing with linear stuff for simplicity for seg in new.get_segments(): self.add_path(seg) else: self.printer.ensure_steppers_enabled() optimize = new.movement != Path.RELATIVE tool_axis = Printer.axis_to_index(self.printer.current_tool) self.native_planner.setAxisConfig(int(self.printer.axis_config)) self.native_planner.queueMove(tuple(new.start_pos), tuple(new.end_pos), new.speed, new.accel, bool(new.cancelable), bool(optimize), bool(new.enable_soft_endstops), bool(new.use_bed_matrix), bool(new.use_backlash_compensation), int(tool_axis), True) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): """ TODO: does this function do anything? Should it be setting the tool axis? """ if ext_nr in range(Printer.MAX_AXES-3): logging.debug("Selecting "+str(ext_nr))
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.printer.path_planner = self self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.home_pos = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.prev = G92Path({"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0}, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setPrintAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter(tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates(tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ /1000.0) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) e.setDirectionInverted(False) self.native_planner.setExtruder(0) self.native_planner.setDriveSystem(Path.axis_config) logging.info("Setting drive system to "+str(Path.axis_config)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread() def get_current_pos(self): """ Get the current pos as a dict """ pos = self.prev.end_pos pos2 = {} for index, axis in enumerate(Path.AXES[:Path.NUM_AXES]): pos2[axis] = pos[index] return pos2 def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def wait_until_sync_event(self): """ Blocks until a PRU sync event occurs """ return (self.native_planner.waitUntilSyncEvent() > 0) def clear_sync_event(self): """ Resumes/Clears a pending sync event """ self.native_planner.clearSyncEvent() def queue_sync_event(self, isBlocking): """ Returns True if a sync event has been queued. False on failure.(use wait_until_done() instead) """ return self.native_planner.queueSyncEvent(isBlocking) def fire_sync_event(self): """ Unclogs any threads waiting for a sync """ def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): self.native_planner.suspend() def resume(self): self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing internal " + str(axis)) path_search = {} path_backoff = {} path_fine_search = {} path_center = {} path_zero = {} speed = Path.home_speed[0] for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue logging.debug("Doing homing for " + str(a)) if Path.home_speed[Path.axis_to_index(a)] < 0: # Search to positive ends path_search[a] = self.travel_length[a] path_center[a] = self.center_offset[a] else: # Search to negative ends path_search[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] backoff_length = -np.sign(path_search[a]) * Path.home_backoff_offset[Path.axis_to_index(a)] path_backoff[a] = backoff_length; path_fine_search[a] = -backoff_length * 1.2; speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) fine_search_speed = min(abs(speed), abs(Path.home_backoff_speed[Path.axis_to_index(a)])) logging.debug("axis: "+str(a)) logging.debug("Search: %s" % path_search) logging.debug("Backoff to: %s" % path_backoff) logging.debug("Fine search: %s" % path_fine_search) logging.debug("Center: %s" % path_center) # Move until endstop is hit p = RelativePath(path_search, speed, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset position to offset p = G92Path(path_center, speed) self.add_path(p) self.wait_until_done() # Back off a bit p = RelativePath(path_backoff, speed, True, False, True, False) self.add_path(p) # Hit the endstop slowly p = RelativePath(path_fine_search, fine_search_speed, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset (final) position to offset p = G92Path(path_center, speed) self.add_path(p) return path_center, speed def _go_to_home(self, axis): """ go to the designated home position do this as a separate call from _home_internal due to delta platforms performing home in cartesian mode """ path_home = {} speed = Path.home_speed[0] for a in axis: path_home[a] = self.home_pos[a] speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) logging.debug("Home: %s" % path_home) # Move to home position p = AbsolutePath(path_home, speed, True, False, False, False) self.add_path(p) self.wait_until_done() return def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if Path.axis_config == Path.AXIS_CONFIG_CORE_XY or \ Path.axis_config == Path.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_DELTA: if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({"X", "Y", "Z"})) # Deltas must home all axes. Path.axis_config = Path.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_DELTA # homing was performed in cartesian mode # need to convert back to delta Az = path_center['X'] Bz = path_center['Y'] Cz = path_center['Z'] z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset xyz = Delta.forward_kinematics2(Az, Bz, Cz) # effector position xyz[2] += z_offset path = {'X':xyz[0], 'Y':xyz[1], 'Z':xyz[2]} p = G92Path(path, speed) self.add_path(p) self.wait_until_done() else: self._home_internal(axis) # go to the designated home position self._go_to_home(axis) # Reset backlash compensation Path.backlash_reset() logging.debug("homing done for " + str(axis)) return def probe(self, z): old_feedrate = self.printer.feed_rate # Save old feedrate speed = Path.home_speed[0] path_back = {"Z": -z} # Move until endstop is hits p = RelativePath(path_back, speed, True) self.wait_until_done() self.add_path(p) self.wait_until_done() self.printer.feed_rate = old_feedrate import struct import mmap PRU_ICSS = 0x4A300000 PRU_ICSS_LEN = 512*1024 RAM2_START = 0x00012000 with open("/dev/mem", "r+b") as f: ddr_mem = mmap.mmap(f.fileno(), PRU_ICSS_LEN, offset=PRU_ICSS) shared = struct.unpack('LLLL', ddr_mem[RAM2_START:RAM2_START+16]) steps_remaining = shared[3] logging.info("Steps remaining : "+str(steps_remaining)) # Update the current Z-position based on the probing delta_z = steps_remaining/Path.steps_pr_meter[2] return delta_z def add_path(self, new): """ Add a path segment to the path planner """ """ This code, and the native planner, needs to be updated for reach. """ # Link to the previous segment in the chain new.set_prev(self.prev) if new.compensation is not None: # Apply a backlash compensation move # CompensationPath(new.compensation, new.speed, False, False, False)) self.native_planner.queueMove(tuple(np.zeros(Path.NUM_AXES)[:4]), tuple(new.compensation[:4]), new.speed, bool(new.cancelable), False) if new.needs_splitting(): path_batch = new.get_delta_segments() # Construct a batch batch_array = np.zeros(shape=(len(path_batch)*2*4),dtype=np.float64) # Change this to reflect NUM_AXIS. for maj_index, path in enumerate(path_batch): for subindex in range(4): # this needs to be NUM_AXIS batch_array[(maj_index * 8) + subindex] = path.start_pos[subindex] batch_array[(maj_index * 8) + 4 + subindex] = path.stepper_end_pos[subindex] self.prev = path self.prev.unlink() # Queue the entire batch at once. self.printer.ensure_steppers_enabled() self.native_planner.queueBatchMove(batch_array, new.speed, bool(new.cancelable), bool(True)) # Do not add the original segment new.unlink() return if not new.is_G92(): self.printer.ensure_steppers_enabled() #push this new segment self.native_planner.queueMove(tuple(new.start_pos[:4]), tuple(new.stepper_end_pos[:4]), new.speed, bool(new.cancelable), bool(new.movement != Path.RELATIVE)) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): if ext_nr in range(Path.NUM_AXES-3): logging.debug("Selecting "+str(ext_nr)) Path.steps_pr_meter[3] = self.printer.steppers[ Path.index_to_axis(ext_nr+3) ].get_steps_pr_meter() self.native_planner.setExtruder(ext_nr)
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.printer.path_planner = self self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.home_pos = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.prev = G92Path({ "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0 }, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None def __init_path_planner(self): self.native_planner = PathPlannerNative( int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setPrintAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ / 1000.0) self.native_planner.setPrintMoveBufferWait( int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime( int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime( int(self.printer.max_buffered_move_time)) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) e.setDirectionInverted(False) self.native_planner.setExtruder(0) self.native_planner.setDriveSystem(Path.axis_config) logging.info("Setting drive system to " + str(Path.axis_config)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread() def get_current_pos(self): """ Get the current pos as a dict """ pos = self.prev.end_pos pos2 = {} for index, axis in enumerate(Path.AXES[:Path.NUM_AXES]): pos2[axis] = pos[index] return pos2 def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def wait_until_sync_event(self): """ Blocks until a PRU sync event occurs """ return (self.native_planner.waitUntilSyncEvent() > 0) def clear_sync_event(self): """ Resumes/Clears a pending sync event """ self.native_planner.clearSyncEvent() def queue_sync_event(self, isBlocking): """ Returns True if a sync event has been queued. False on failure.(use wait_until_done() instead) """ return self.native_planner.queueSyncEvent(isBlocking) def fire_sync_event(self): """ Unclogs any threads waiting for a sync """ def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): self.native_planner.suspend() def resume(self): self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing internal " + str(axis)) path_search = {} path_backoff = {} path_fine_search = {} path_center = {} path_zero = {} speed = Path.home_speed[0] for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue logging.debug("Doing homing for " + str(a)) if Path.home_speed[Path.axis_to_index(a)] < 0: # Search to positive ends path_search[a] = self.travel_length[a] path_center[a] = self.center_offset[a] else: # Search to negative ends path_search[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] backoff_length = -np.sign( path_search[a]) * Path.home_backoff_offset[Path.axis_to_index( a)] path_backoff[a] = backoff_length path_fine_search[a] = -backoff_length * 1.2 speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) fine_search_speed = min( abs(speed), abs(Path.home_backoff_speed[Path.axis_to_index(a)])) logging.debug("axis: " + str(a)) logging.debug("Search: %s" % path_search) logging.debug("Backoff to: %s" % path_backoff) logging.debug("Fine search: %s" % path_fine_search) logging.debug("Center: %s" % path_center) # Move until endstop is hit p = RelativePath(path_search, speed, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset position to offset p = G92Path(path_center, speed) self.add_path(p) self.wait_until_done() # Back off a bit p = RelativePath(path_backoff, speed, True, False, True, False) self.add_path(p) # Hit the endstop slowly p = RelativePath(path_fine_search, fine_search_speed, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset (final) position to offset p = G92Path(path_center, speed) self.add_path(p) return path_center, speed def _go_to_home(self, axis): """ go to the designated home position do this as a separate call from _home_internal due to delta platforms performing home in cartesian mode """ path_home = {} speed = Path.home_speed[0] for a in axis: path_home[a] = self.home_pos[a] speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) logging.debug("Home: %s" % path_home) # Move to home position # p = AbsolutePath(path_home, speed, True, False, False, False) # self.add_path(p) # self.wait_until_done() return def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if Path.axis_config == Path.AXIS_CONFIG_CORE_XY or \ Path.axis_config == Path.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_DELTA: if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({"X", "Y", "Z" })) # Deltas must home all axes. Path.axis_config = Path.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_DELTA # homing was performed in cartesian mode # need to convert back to delta Az = path_center['X'] Bz = path_center['Y'] Cz = path_center['Z'] z_offset = Delta.vertical_offset(Az, Bz, Cz) # vertical offset xyz = Delta.forward_kinematics2(Az, Bz, Cz) # effector position xyz[2] += z_offset path = {'X': xyz[0], 'Y': xyz[1], 'Z': xyz[2]} p = G92Path(path, speed) self.add_path(p) self.wait_until_done() # For scar, switch also to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_SCARA: # TODO: implement individual homing ( needs to take care of position after homing) if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({ "X", "Y", "Z" })) # For now Scaras must home all axes. Than can changed Path.axis_config = Path.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_SCARA # homing was performed in cartesian mode # need to convert back to delta A = path_center['X'] B = path_center['Y'] C = path_center['Z'] #z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset # xyz = Scara.inverse_kinematics(A, B, C) # effector position # logging.debug("HomeXYZ: %s", xyz) #xyz[2] += z_offset # don't cpnvert home_pos to effector spac # home offset is defined in cartesian space # xyz = np.array([path_center['X'], path_center['Y'], path_center['Z']]) #path = {'X':xyz[0], 'Y':xyz[1], 'Z':xyz[2]} path = {A, B, C} p = G92Path(path, speed) self.add_path(p) self.wait_until_done() else: self._home_internal(axis) # go to the designated home position self._go_to_home(axis) # Reset backlash compensation Path.backlash_reset() logging.debug("homing done for " + str(axis)) return def probe(self, z): old_feedrate = self.printer.feed_rate # Save old feedrate speed = Path.home_speed[0] path_back = {"Z": -z} # Move until endstop is hits p = RelativePath(path_back, speed, True) self.wait_until_done() self.add_path(p) self.wait_until_done() self.printer.feed_rate = old_feedrate import struct import mmap PRU_ICSS = 0x4A300000 PRU_ICSS_LEN = 512 * 1024 RAM2_START = 0x00012000 with open("/dev/mem", "r+b") as f: ddr_mem = mmap.mmap(f.fileno(), PRU_ICSS_LEN, offset=PRU_ICSS) shared = struct.unpack('LLLL', ddr_mem[RAM2_START:RAM2_START + 16]) steps_remaining = shared[3] logging.info("Steps remaining : " + str(steps_remaining)) # Update the current Z-position based on the probing delta_z = steps_remaining / Path.steps_pr_meter[2] return delta_z def add_path(self, new): """ Add a path segment to the path planner """ """ This code, and the native planner, needs to be updated for reach. """ #rpdb2.start_embedded_debugger( #fAllowUnencrypted = True, #"laydrop", #fAllowRemote = True, #timeout = rpdb2.TIMEOUT_FIVE_MINUTES, #fDebug = True #) # Link to the previous segment in the chain new.set_prev(self.prev) logging.debug("after set.prev") if new.compensation is not None: # Apply a backlash compensation move # CompensationPath(new.compensation, new.speed, False, False, False)) self.native_planner.queueMove(tuple(np.zeros(Path.NUM_AXES)[:4]), tuple(new.compensation[:4]), new.speed, bool(new.cancelable), False) logging.debug("Just before calc segments") if new.needs_splitting(): path_batch = new.get_delta_segments() #logging.debug("Batch Array %s", path_batch) # Construct a batch batch_array = np.zeros( shape=(len(path_batch) * 2 * 4), dtype=np.float64) # Change this to reflect NUM_AXIS. for maj_index, path in enumerate(path_batch): for subindex in range(3): # this needs to be NUM_AXIS if subindex == 0: batch_array[(maj_index * 8)] = (path.start_ABC[0] + path.start_ABC[1]) / 1000 batch_array[(maj_index * 8) + 4] = (path.end_ABC[0] + path.end_ABC[1]) / 1000 else: batch_array[(maj_index * 8) + subindex] = path.start_ABC[subindex] / 1000 batch_array[(maj_index * 8) + 4 + subindex] = path.end_ABC[subindex] / 1000 batch_array[(maj_index * 8) + 3] = path.start_ABC[2] / 1000 batch_array[(maj_index * 8) + 4 + 3] = path.end_ABC[2] / 1000 logging.debug( "ABC von:%9.5f,%9.5f,%9.5f bis:%9.5f,%9.5f,%9.5f" % (path.start_ABC[0], path.start_ABC[1], path.start_ABC[2], path.end_ABC[0], path.end_ABC[1], path.end_ABC[2])) self.prev = path self.prev.unlink() logging.debug("Speed: %9.5f" % new.speed) # Queue the entire batch at once. self.printer.ensure_steppers_enabled() self.native_planner.queueBatchMove(batch_array, new.speed, bool(new.cancelable), bool(True)) logging.debug("Batch Array %s", batch_array) # Do not add the original segment new.unlink() return if not new.is_G92(): # rpdb2.start_embedded_debugger( # "laydrop", # fAllowUnencrypted = True, # fAllowRemote = True, # timeout = rpdb2.TIMEOUT_FIVE_MINUTES, # fDebug = True # ) self.printer.ensure_steppers_enabled() #push this new segment start = np.array([ new.start_ABC[0] / 1000, new.start_ABC[1] / 1000, new.start_ABC[2] / 1000, new.start_ABC[2] / 1000 ]) end = np.array([ new.end_ABC[0] / 1000, new.end_ABC[1] / 1000, new.end_ABC[2] / 1000, new.end_ABC[2] / 1000 ]) # logging.critical("L-Start:%02d L-End;%02d"%(len(start),len(end))) self.native_planner.queueMove(tuple(start), tuple(end), new.speed, bool(new.cancelable), bool(new.movement != Path.RELATIVE)) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): if ext_nr in range(Path.NUM_AXES - 3): logging.debug("Selecting " + str(ext_nr)) Path.steps_pr_meter[3] = self.printer.steppers[Path.index_to_axis( ext_nr + 3)].get_steps_pr_meter() self.native_planner.setExtruder(ext_nr)
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.printer.path_planner = self self.travel_length = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.center_offset = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.home_pos = {"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0} self.prev = G92Path({"X": 0, "Y": 0, "Z": 0, "E": 0, "H": 0, "A": 0, "B": 0, "C": 0}, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None def __init_path_planner(self): self.native_planner = PathPlannerNative(int(self.printer.move_cache_size)) fw0 = self.pru_firmware.get_firmware(0) fw1 = self.pru_firmware.get_firmware(1) if fw0 is None or fw1 is None: return self.native_planner.initPRU(fw0, fw1) self.native_planner.setAcceleration(tuple(Path.acceleration)) self.native_planner.setAxisStepsPerMeter(tuple(Path.steps_pr_meter)) self.native_planner.setMaxSpeeds(tuple(Path.max_speeds)) self.native_planner.setMinSpeeds(tuple(Path.min_speeds)) self.native_planner.setJerks(tuple(Path.jerks)) self.native_planner.setPrintMoveBufferWait(int(self.printer.print_move_buffer_wait)) self.native_planner.setMinBufferedMoveTime(int(self.printer.min_buffered_move_time)) self.native_planner.setMaxBufferedMoveTime(int(self.printer.max_buffered_move_time)) self.printer.plugins.path_planner_initialized(self) self.native_planner.runThread() def restart(self): self.native_planner.stopThread(True) self.__init_path_planner() def update_steps_pr_meter(self): """ Update steps pr meter from the path """ self.native_planner.setAxisStepsPerMeter(tuple(Path.steps_pr_meter)) def get_current_pos(self): """ Get the current pos as a dict """ pos = self.prev.end_pos pos2 = {} for index, axis in enumerate(Path.AXES[:Path.MAX_AXES]): pos2[axis] = pos[index] return pos2 def get_extruder_pos(self, ext_nr): """ Return the current position of this extruder """ return self.prev.end_pos[3+ext_nr] def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def wait_until_sync_event(self): """ Blocks until a PRU sync event occurs """ return (self.native_planner.waitUntilSyncEvent() > 0) def clear_sync_event(self): """ Resumes/Clears a pending sync event """ self.native_planner.clearSyncEvent() def queue_sync_event(self, isBlocking): """ Returns True if a sync event has been queued. False on failure.(use wait_until_done() instead) """ return self.native_planner.queueSyncEvent(isBlocking) def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): ''' Temporary pause of planner ''' self.native_planner.suspend() def resume(self): ''' resume a paused planner ''' self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing internal " + str(axis)) path_search = {} path_backoff = {} path_fine_search = {} path_center = {} path_zero = {} speed = Path.home_speed[0] # TODO: speed for each axis accel = self.printer.acceleration[0] # TODO: accel for each axis for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue logging.debug("Doing homing for " + str(a)) if Path.home_speed[Path.axis_to_index(a)] < 0: # Search to positive ends path_search[a] = self.travel_length[a] path_center[a] = self.center_offset[a] else: # Search to negative ends path_search[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] backoff_length = -np.sign(path_search[a]) * Path.home_backoff_offset[Path.axis_to_index(a)] path_backoff[a] = backoff_length; path_fine_search[a] = -backoff_length * 1.2; speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) fine_search_speed = min(abs(speed), abs(Path.home_backoff_speed[Path.axis_to_index(a)])) logging.debug("Search: %s" % path_search) logging.debug("Backoff to: %s" % path_backoff) logging.debug("Fine search: %s" % path_fine_search) logging.debug("Center: %s" % path_center) # Move until endstop is hit p = RelativePath(path_search, speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() logging.debug("Coarse search done!") # Reset position to offset p = G92Path(path_center, speed) self.add_path(p) self.wait_until_done() # Back off a bit p = RelativePath(path_backoff, speed, accel, True, False, True, False) self.add_path(p) # Hit the endstop slowly p = RelativePath(path_fine_search, fine_search_speed, accel, True, False, True, False) self.add_path(p) self.wait_until_done() # Reset (final) position to offset p = G92Path(path_center, speed) self.add_path(p) return path_center, speed def _go_to_home(self, axis): """ go to the designated home position do this as a separate call from _home_internal due to delta platforms performing home in cartesian mode """ path_home = {} speed = Path.home_speed[0] accel = self.printer.acceleration[0] for a in axis: path_home[a] = self.home_pos[a] speed = min(abs(speed), abs(Path.home_speed[Path.axis_to_index(a)])) logging.debug("Home: %s" % path_home) # Move to home position p = AbsolutePath(path_home, speed, accel, True, False, False, False) self.add_path(p) self.wait_until_done() # Due to rounding errors, we explicitly set the found # position to the right value. # Reset (final) position to offset p = G92Path(path_home, speed) self.add_path(p) return def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if Path.axis_config == Path.AXIS_CONFIG_CORE_XY or \ Path.axis_config == Path.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_DELTA: if 0 < len({"X", "Y", "Z"}.intersection(set(axis))) < 3: axis = list(set(axis).union({"X", "Y", "Z"})) # Deltas must home all axes. Path.axis_config = Path.AXIS_CONFIG_XY path_center, speed = self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_DELTA # homing was performed in cartesian mode # need to convert back to delta Az = path_center['X'] Bz = path_center['Y'] Cz = path_center['Z'] z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset xyz = Delta.forward_kinematics2(Az, Bz, Cz) # effector position xyz[2] += z_offset path = {'X':xyz[0], 'Y':xyz[1], 'Z':xyz[2]} p = G92Path(path, speed) self.add_path(p) self.wait_until_done() else: # AXIS_CONFIG_XY self._home_internal(axis) # go to the designated home position self._go_to_home(axis) # Reset backlash compensation Path.backlash_reset() logging.debug("homing done for " + str(axis)) return def probe(self, z, speed, accel): self.wait_until_done() # Move until endstop is hits self.printer.ensure_steppers_enabled() #push this new segment start = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) steps = np.ceil(z*Path.steps_pr_meter[2]) z_dist = steps/Path.steps_pr_meter[2] # select end point based on the type of bot if Path.axis_config == Path.AXIS_CONFIG_DELTA: end = (-z_dist, -z_dist, -z_dist, 0.0, 0.0, 0.0, 0.0, 0.0) else: # AXIS_CONFIG_XY, AXIS_CONFIG_H_BELT, AXIS_CONFIG_CORE_XY end = (0.0, 0.0, -z_dist, 0.0, 0.0, 0.0, 0.0, 0.0) logging.debug("Steps total: "+str(steps)) self.native_planner.queueMove(start, end, speed, accel, True, True) self.wait_until_done() # TODO: Move this to PruInterface.py import struct import mmap PRU_ICSS = 0x4A300000 PRU_ICSS_LEN = 512*1024 RAM2_START = 0x00012000 with open("/dev/mem", "r+b") as f: ddr_mem = mmap.mmap(f.fileno(), PRU_ICSS_LEN, offset=PRU_ICSS) shared = struct.unpack('LLLL', ddr_mem[RAM2_START:RAM2_START+16]) steps_remaining = shared[3] logging.debug("Steps remaining : "+str(steps_remaining)) # Calculate how many steps the Z axis moved steps -= steps_remaining z_dist = steps/Path.steps_pr_meter[2] start = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # select end point based on the type of bot if Path.axis_config == Path.AXIS_CONFIG_DELTA: end = (z_dist, z_dist, z_dist, 0.0, 0.0, 0.0, 0.0, 0.0) else: # AXIS_CONFIG_XY, AXIS_CONFIG_H_BELT, AXIS_CONFIG_CORE_XY end = (0.0, 0.0, z_dist, 0.0, 0.0, 0.0, 0.0, 0.0) self.native_planner.queueMove(start, end, speed, accel, True, False) self.wait_until_done() return steps/Path.steps_pr_meter[2] def add_path(self, new): """ Add a path segment to the path planner """ """ This code, and the native planner, needs to be updated for reach. """ # Link to the previous segment in the chain #logging.debug("Adding "+str(new)) new.set_prev(self.prev) if new.compensation: # Apply a backlash compensation move self.native_planner.queueMove(tuple(np.zeros(Path.MAX_AXES)), tuple(new.compensation), new.speed, new.accel, bool(new.cancelable), False) if new.needs_splitting(): path_batch = new.get_segments() # Construct a batch batch_array = np.zeros(shape=(len(path_batch)*2*Path.MAX_AXES), dtype=np.float64) # Change this to reflect NUM_AXIS. for maj_index, path in enumerate(path_batch): for subindex in range(Path.MAX_AXES): # this needs to be NUM_AXIS batch_array[(maj_index * Path.MAX_AXES * 2) + subindex] = path.start_pos[subindex] batch_array[(maj_index * Path.MAX_AXES * 2) + Path.MAX_AXES + subindex] = path.stepper_end_pos[subindex] self.prev = path self.prev.unlink() # Queue the entire batch at once. self.printer.ensure_steppers_enabled() self.native_planner.queueBatchMove(batch_array, new.speed, new.accel, bool(new.cancelable), True) # Do not add the original segment new.unlink() return if not new.is_G92(): self.printer.ensure_steppers_enabled() #push this new segment start = tuple(new.start_pos) end = tuple(new.stepper_end_pos) can = bool(new.cancelable) rel = bool(new.movement != Path.RELATIVE) #logging.debug("Queueing "+str(start)+" "+str(end)+" "+str(new.speed)+" "+str(new.accel)+" "+str(can)+" "+str(rel)) self.native_planner.queueMove(tuple(new.start_pos), tuple(new.stepper_end_pos), new.speed, new.accel, bool(new.cancelable), bool(new.movement != Path.RELATIVE)) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): if ext_nr in range(Path.MAX_AXES-3): logging.debug("Selecting "+str(ext_nr))
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.prev = G92Path({ "X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0 }, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None def __init_path_planner(self): self.native_planner = PathPlannerNative() self.native_planner.initPRU(self.pru_firmware.get_firmware(0), self.pru_firmware.get_firmware(1)) self.native_planner.setPrintAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration( tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ / 1000.0) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) self.native_planner.setExtruder(0) self.native_planner.runThread() def get_current_pos(self): """ Get the current pos as a dict """ pos = np.zeros(Path.NUM_AXES) if self.prev: pos = self.prev.end_pos pos2 = {} for index, axis in enumerate(Path.AXES): pos2[axis] = pos[index] return pos2 def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): self.native_planner.suspend() def resume(self): self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing " + str(axis)) path_back = {} path_center = {} path_zero = {} speed = Path.home_speed[0] for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue path_back[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] path_zero[a] = 0 speed = min(speed, Path.home_speed[Path.axis_to_index(a)]) # Move until endstop is hit p = RelativePath(path_back, speed, True) self.add_path(p) # Reset position to offset p = G92Path(path_center, speed) self.add_path(p) # Move to offset p = AbsolutePath(path_zero, speed) self.add_path(p) self.wait_until_done() logging.debug("homing done for " + str(axis)) def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if Path.axis_config == Path.AXIS_CONFIG_CORE_XY or \ Path.axis_config == Path.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_DELTA: Path.axis_config = Path.AXIS_CONFIG_XY self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_DELTA else: self._home_internal(axis) def add_path(self, new): """ Add a path segment to the path planner """ # Link to the previous segment in the chain new.set_prev(self.prev) if not new.is_G92(): self.printer.ensure_steppers_enabled() #push this new segment self.native_planner.queueMove(tuple(new.delta[:4]), tuple(new.num_steps[:4]), new.speed, bool(new.cancelable), bool(new.movement != Path.RELATIVE)) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): if ext_nr in [0, 1]: if ext_nr == 0: Path.steps_pr_meter[3] = self.printer.steppers[ "E"].get_steps_pr_meter() elif ext_nr == 1: Path.steps_pr_meter[3] = self.printer.steppers[ "H"].get_steps_pr_meter() self.native_planner.setExtruder(ext_nr) def queue_move(self, path): """ start of Python impl of queue_move """ # Not working! path.primay_axis = np.max(path.delta) path.diff = path.delta * (1.0 / path.steps_pr_meter) path.steps_remaining = path.delta[path.primary_axis] path.xyz_dist = np.sqrt(np.dot(path.delta[:3], path.delta[:3])) path.distance = np.max(path.xyz_dist, path.diff[4]) calculate_move(path) def calculate_move(self, path): """ Start of Python impl of calculate move """ # Not working! axis_interval[4] speed = max(minimumSpeed, path.speed) if path.is_x_or_y_move() else path.speed # time is in ticks path.time_in_ticks = time_for_move = F_CPU * path.distance / speed # Compute the slowest allowed interval (ticks/step), so maximum # feedrate is not violated axis_interval = abs( path.diff) * F_CPU / Path.max_speeds * path.steps_remaining limit_interval = max(np.max(axis_interval), time_for_move / path.steps_remaining) path.full_interval = limit_interval # new time at full speed = limitInterval*p->stepsRemaining [ticks] time_for_move = limit_interval * path.steps_remaining inv_time_s = F_CPU / time_for_move axis_interval = time_for_move / path.delta path.speed = sign(path.delta) * axis_diff * inv_time_s
class PathPlanner: def __init__(self, printer, pru_firmware): """ Init the planner """ self.printer = printer self.steppers = printer.steppers self.pru_firmware = pru_firmware self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0} self.prev = G92Path({"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0}, 0) self.prev.set_prev(None) if pru_firmware: self.__init_path_planner() else: self.native_planner = None def __init_path_planner(self): self.native_planner = PathPlannerNative() self.native_planner.initPRU(self.pru_firmware.get_firmware(0), self.pru_firmware.get_firmware(1)) self.native_planner.setPrintAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setTravelAcceleration(tuple([float(self.printer.acceleration[i]) for i in range(3)])) self.native_planner.setAxisStepsPerMeter( tuple([long(Path.steps_pr_meter[i]) for i in range(3)])) self.native_planner.setMaxFeedrates( tuple([float(Path.max_speeds[i]) for i in range(3)])) self.native_planner.setMaxJerk(self.printer.maxJerkXY / 1000.0, self.printer.maxJerkZ /1000.0) #Setup the extruders for i in range(Path.NUM_AXES - 3): e = self.native_planner.getExtruder(i) e.setMaxFeedrate(Path.max_speeds[i + 3]) e.setPrintAcceleration(self.printer.acceleration[i + 3]) e.setTravelAcceleration(self.printer.acceleration[i + 3]) e.setMaxStartFeedrate(self.printer.maxJerkEH / 1000) e.setAxisStepsPerMeter(long(Path.steps_pr_meter[i + 3])) self.native_planner.setExtruder(0) self.native_planner.runThread() def get_current_pos(self): """ Get the current pos as a dict """ pos = np.zeros(Path.NUM_AXES) if self.prev: pos = self.prev.end_pos pos2 = {} for index, axis in enumerate(Path.AXES): pos2[axis] = pos[index] return pos2 def wait_until_done(self): """ Wait until the queue is empty """ self.native_planner.waitUntilFinished() def force_exit(self): self.native_planner.stopThread(True) def emergency_interrupt(self): """ Stop in emergency any moves. """ # Note: This method has to be thread safe as it can be called from the # command thread directly or from the command queue thread self.native_planner.suspend() for name, stepper in self.printer.steppers.iteritems(): stepper.set_disabled(True) #Create a new path planner to have everything clean when it restarts self.native_planner.stopThread(True) self.__init_path_planner() def suspend(self): self.native_planner.suspend() def resume(self): self.native_planner.resume() def _home_internal(self, axis): """ Private method for homing a set or a single axis """ logging.debug("homing " + str(axis)) path_back = {} path_center = {} path_zero = {} speed = Path.home_speed[0] for a in axis: if not self.printer.steppers[a].has_endstop: logging.debug("Skipping homing for " + str(a)) continue path_back[a] = -self.travel_length[a] path_center[a] = -self.center_offset[a] path_zero[a] = 0 speed = min(speed, Path.home_speed[Path.axis_to_index(a)]) # Move until endstop is hit p = RelativePath(path_back, speed, True) self.add_path(p) # Reset position to offset p = G92Path(path_center, speed) self.add_path(p) # Move to offset p = AbsolutePath(path_zero, speed) self.add_path(p) self.wait_until_done() logging.debug("homing done for " + str(axis)) def home(self, axis): """ Home the given axis using endstops (min) """ logging.debug("homing " + str(axis)) # Home axis for core X,Y and H-Belt independently to avoid hardware # damages. if Path.axis_config == Path.AXIS_CONFIG_CORE_XY or \ Path.axis_config == Path.AXIS_CONFIG_H_BELT: for a in axis: self._home_internal(a) # For delta, switch to cartesian when homing elif Path.axis_config == Path.AXIS_CONFIG_DELTA: Path.axis_config = Path.AXIS_CONFIG_XY self._home_internal(axis) Path.axis_config = Path.AXIS_CONFIG_DELTA else: self._home_internal(axis) def add_path(self, new): """ Add a path segment to the path planner """ # Link to the previous segment in the chain new.set_prev(self.prev) if not new.is_G92(): self.printer.ensure_steppers_enabled() #push this new segment self.native_planner.queueMove(tuple(new.delta[:4]), tuple(new.num_steps[:4]), new.speed, bool(new.cancelable), bool(new.movement != Path.RELATIVE)) self.prev = new self.prev.unlink() # We don't want to store the entire print # in memory, so we keep only the last path. def set_extruder(self, ext_nr): if ext_nr in [0, 1]: if ext_nr == 0: Path.steps_pr_meter[3] = self.printer.steppers[ "E"].get_steps_pr_meter() elif ext_nr == 1: Path.steps_pr_meter[3] = self.printer.steppers[ "H"].get_steps_pr_meter() self.native_planner.setExtruder(ext_nr) def queue_move(self, path): """ start of Python impl of queue_move """ # Not working! path.primay_axis = np.max(path.delta) path.diff = path.delta * (1.0 / path.steps_pr_meter) path.steps_remaining = path.delta[path.primary_axis] path.xyz_dist = np.sqrt(np.dot(path.delta[:3], path.delta[:3])) path.distance = np.max(path.xyz_dist, path.diff[4]) calculate_move(path) def calculate_move(self, path): """ Start of Python impl of calculate move """ # Not working! axis_interval[4]; speed = max(minimumSpeed, path.speed) if path.is_x_or_y_move() else path.speed # time is in ticks path.time_in_ticks = time_for_move = F_CPU * path.distance / speed # Compute the slowest allowed interval (ticks/step), so maximum # feedrate is not violated axis_interval = abs( path.diff) * F_CPU / Path.max_speeds * path.steps_remaining limit_interval = max(np.max(axis_interval), time_for_move / path.steps_remaining) path.full_interval = limit_interval # new time at full speed = limitInterval*p->stepsRemaining [ticks] time_for_move = limit_interval * path.steps_remaining; inv_time_s = F_CPU / time_for_move; axis_interval = time_for_move / path.delta; path.speed = sign(path.delta) * axis_diff * inv_time_s;