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;
