def __init__(self, steppers, pru_firmware):
self.steppers = steppers
self.pru = Pru(pru_firmware) # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0}
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
self.t.daemon = True
self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0}
self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0}
if __name__ != '__main__':
self.t.start()
def __init__(self, steppers, current_pos):
self.steppers = steppers
self.pru = Pru() # Make the PRU
self.paths = Queue.Queue(30) # Make a queue of paths
self.current_pos = current_pos # Current position in (x, y, z, e)
self.running = True # Yes, we are running
self.pru_data = defaultdict(int)
self.t = Thread(target=self._do_work) # Make the thread
self.t.start()
def __init__(self, steppers, current_pos):
self.steppers = steppers
self.pru = Pru() # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = current_pos # Current position in (x, y, z, e)
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
if __name__ != '__main__':
self.t.start()
def __init__(self, steppers, pru_firmware):
self.steppers = steppers
self.pru = Pru(pru_firmware) # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = {"X":0.0, "Y":0.0, "Z":0.0, "E":0.0,"H":0.0}
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
self.t.daemon = True
self.travel_length = {"X":0.0, "Y":0.0, "Z":0.0}
self.center_offset = {"X":0.0, "Y":0.0, "Z":0.0}
if __name__ != '__main__':
self.t.start()
class Path_planner:
''' Init the planner '''
def __init__(self, steppers, current_pos):
self.steppers = steppers
self.pru = Pru() # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = current_pos # Current position in (x, y, z, e)
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
if __name__ != '__main__':
self.t.start()
''' Set the acceleration used ''' # Fix me, move this to path
def set_acceleration(self, acceleration):
self.acceleration = acceleration
''' Add a path segment to the path planner '''
def add_path(self, new):
if not new.is_G92():
if hasattr(self, 'prev'):
self.prev.set_next(new)
new.set_prev(self.prev)
self.prev = new
self.paths.put(new)
''' Return the number of paths currently on queue '''
def nr_of_paths(self):
return self.paths.qsize()
''' Set position for an axis '''
def set_pos(self, axis, val):
self.current_pos[axis] = val
def wait_until_done(self):
'''Wait until planner is done'''
self.paths.join()
self.pru.wait_until_done()
def _do_work(self):
""" This loop pops a path, sends it to the PRU and waits for an event """
while self.running:
self.do_work()
def do_work(self):
""" This is just a separate function so the test at the bottom will pass """
path = self.paths.get() # Get the last path added
path.set_global_pos(self.current_pos.copy()) # Set the global position of the printer
if path.is_G92(): # Only set the position of the axes
for axis, pos in path.get_pos().iteritems(): # Run through all the axes in the path
self.set_pos(axis, pos)
self.paths.task_done()
return
all_data = {}
for axis in path.get_axes(): # Run through all the axes in the path
stepper = self.steppers[axis] # Get a handle of the stepper
data = self._make_data(path, axis)
if len(data[0]) > 0:
if len(self.pru_data) == 0:
self.pru_data = zip(*data)
else:
self.pru_data = self._braid_data(self.pru_data, zip(*data))
#self._braid_data1(self.pru_data, zip(*data))
while len(self.pru_data) > 0:
data = self.pru_data[0:0x20000/8]
del self.pru_data[0:0x20000/8]
if len(self.pru_data) > 0:
logging.debug("Long path segment is cut. remaining: "+str(len(self.pru_data)))
while not self.pru.has_capacity_for(len(data)*8):
#logging.debug("Pru full")
time.sleep(1)
self.pru.add_data(zip(*data))
self.pru.commit_data() # Commit data to ddr
self.pru_data = []
self.paths.task_done()
path.unlink() # Remove reference to enable garbage collection
path = None
def _braid_data(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
return braid.braid_data_c(data1, data2)
def _braid_data1(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
line = 0
(pin1, dly1) = data1[line]
(pin2, dly2) = data2.pop(0)
while True:
dly = min(dly1, dly2)
dly1 -= dly
dly2 -= dly
try:
if dly1 == 0 and dly2 == 0:
data1[line] = (pin1+pin2, dly)
(pin1, dly1) = data1[line+1]
(pin2, dly2) = data2.pop(0)
elif dly1 == 0:
data1[line] = (pin1+pin2, dly)
(pin1, dly1) = data1[line+1]
elif dly2 == 0:
data1.insert(line, (pin1+pin2, dly))
(pin2, dly2) = data2.pop(0)
line += 1
except IndexError, e:
break
if dly2 > 0:
data1[line] = (data1[line][0], data1[line][1]+dly2)
elif dly1 > 0:
data1[line] = (data1[line][0], data1[line][1]+dly1)
data1.pop(line+1)
while len(data2) > 0:
line += 1
(pin2, dly2) = data2.pop(0)
data1.append((pin2+pin1, dly2))
while len(data1) > line+1:
line += 1
(pin1, dly1) = data1[line]
data1[line] = (pin2+pin1, dly1)
class PathPlanner:
''' Init the planner '''
def __init__(self, steppers, pru_firmware):
self.steppers = steppers
self.pru = Pru(pru_firmware) # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = {"X": 0.0, "Y": 0.0, "Z": 0.0, "E": 0.0, "H": 0.0}
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
self.t.daemon = True
self.travel_length = {"X": 0.0, "Y": 0.0, "Z": 0.0}
self.center_offset = {"X": 0.0, "Y": 0.0, "Z": 0.0}
if __name__ != '__main__':
self.t.start()
''' Set travel length (printer size) for all axis '''
def set_travel_length(self, travel):
self.travel_length = travel
''' Set offset of printer center for all axis '''
def set_center_offset(self, offset):
self.center_offset = offset
''' Set the acceleration used ''' # Fix me, move this to path
def set_acceleration(self, acceleration):
self.acceleration = acceleration
''' Home the given axis using endstops (min) '''
def home(self, axis):
if axis == "E" or axis == "H":
return
logging.debug("homing " + axis)
p = Path({axis: -self.travel_length[axis]}, 0.01, "RELATIVE", False,
False)
p.set_homing_feedrate()
#p.set_max_speed(p.get_max_speed()*0.2)
self.add_path(p)
path = Path({axis: -self.center_offset[axis]}, 0.01, "G92")
self.add_path(path)
p = Path({axis: 0}, 0.01, "ABSOLUTE")
p.set_homing_feedrate()
#p.set_max_speed(p.get_max_speed()*0.2)
self.add_path(p)
self.wait_until_done()
logging.debug("homing done for " + axis)
''' Add a path segment to the path planner '''
def add_path(self, new):
if not new.is_G92():
if hasattr(self, 'prev'):
self.prev.set_next(new)
new.set_prev(self.prev)
self.prev = new
self.paths.put(new)
''' Return the number of paths currently on queue '''
def nr_of_paths(self):
return self.paths.qsize()
''' Set position for an axis '''
def _set_pos(self, axis, val):
self.current_pos[axis] = val
def wait_until_done(self):
'''Wait until planner is done'''
self.paths.join()
self.pru.wait_until_done()
def _do_work(self):
""" This loop pops a path, sends it to the PRU and waits for an event """
while self.running:
self.do_work()
def do_work(self):
""" This is just a separate function so the test at the bottom will pass """
path = self.paths.get() # Get the last path added
path.set_global_pos(
self.current_pos.copy()) # Set the global position of the printer
if path.is_G92(): # Only set the position of the axes
for axis, pos in path.get_pos().iteritems(
): # Run through all the axes in the path
self._set_pos(axis, pos)
self.paths.task_done()
return
for axis in path.get_axes(): # Run through all the axes in the path
data = self._make_data(path, axis)
if len(data[0]) > 0:
if len(self.pru_data) == 0:
self.pru_data = zip(*data)
else:
#self._braid_data1(self.pru_data, zip(*data))
self.pru_data = self._braid_data(self.pru_data, zip(*data))
while len(self.pru_data) > 0:
data = self.pru_data[0:0x10000 / 8]
del self.pru_data[0:0x10000 / 8]
if len(self.pru_data) > 0:
logging.debug("Long path segment is cut. remaining: " +
str(len(self.pru_data)))
while not self.pru.has_capacity_for(len(data) * 8):
#logging.debug("Pru full")
time.sleep(0.5)
self.pru.add_data(zip(*data))
self.pru.commit_data() # Commit data to ddr
self.pru_data = []
self.paths.task_done()
path.unlink() # Remove reference to enable garbage collection
def emergency_interrupt(self):
self.pru.emergency_interrupt()
while True:
try:
path = self.paths.get(block=False)
if path != None:
self.paths.task_done()
path.unlink()
except Queue.Empty:
break
def _braid_data(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
return braid.braid_data_c(
data1, data2) # Use the Optimized C-function foir this.
def _braid_data1(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
line = 0
(pin1, dir1, o1, dly1) = data1[line]
(pin2, dir2, o2, dly2) = data2.pop(0)
while True:
dly = min(dly1, dly2)
dly1 -= dly
dly2 -= dly
try:
if dly1 == 0 and dly2 == 0:
data1[line] = (pin1 | pin2, dir1 | dir2, o1 | o2, dly)
(pin1, dir1, o1, dly1) = data1[line + 1]
(pin2, dir2, o2, dly2) = data2.pop(0)
elif dly1 == 0:
data1[line] = (pin1, dir1, o1, dly)
(pin1, dir1, o1, dly1) = data1[line + 1]
elif dly2 == 0:
data1.insert(line, (pin2, dir2, o2, dly))
(pin2, dir2, o2, dly2) = data2.pop(0)
line += 1
except IndexError:
break
if dly2 > 0:
#data1[line] = (data1[line][0],data1[line][1],data1[line][2], data1[line][3]+dly2)
data1.append((pin2, dir2, o2, dly2))
line += 1
elif dly1 > 0:
data1[line] = (data1[line][0], data1[line][1], data1[line][2],
data1[line][3] + dly1)
#data1.pop(line+1)
while len(data2) > 0:
line += 1
(pin2, dir2, o2, dly2) = data2.pop(0)
data1.append((pin2, dir2, o2, dly2))
#while len(data1) > line+1:
# line += 1
# (pin1, dir1,o1, dly1) = data1[line]
# data1[line] = (pin2|pin1,dir1 | dir2,o1 | o2, dly1)
''' Join the thread '''
def exit(self):
self.running = False
self.pru.join()
self.t.join()
def force_exit(self):
self.running = False
self.pru.force_exit()
''' Make the data for the PRU or steppers '''
def _make_data(self, path, axis):
stepper = self.steppers[axis]
steps_pr_meter = stepper.get_steps_pr_meter()
vec = path.get_axis_length(axis) # Total travel distance
num_steps = int(abs(vec) * steps_pr_meter) # Number of steps to tick
if num_steps == 0:
return ([], [])
step_pin = stepper.get_step_pin() # Get the step pin
dir_pin = stepper.get_dir_pin() # Get the direction pin
#if stepper.get_direction() > 0:
dir_pin = 0 if vec < 0 else dir_pin # Disable the dir-pin if we are going backwards
#else:
# dir_pin = 0 if vec >= 0 else dir_pin
step_pins = [step_pin] * num_steps # Make the pin states
dir_pins = [dir_pin] * num_steps
option_pins = [1 if path.is_cancellable() else 0] * num_steps
s = abs(path.get_axis_length(axis)) # Get the length of the vector
ratio = path.get_axis_ratio(
axis) # Ratio is the length of this axis to the total length
Vm = path.get_max_speed() * ratio # The travelling speed in m/s
a = self.acceleration * ratio # Accelleration in m/s/s
ds = 1.0 / steps_pr_meter # Delta S, distance in meters travelled pr step.
#logging.debug('Start speed '+str(path.get_start_speed()))
#logging.debug('End speed '+str(path.get_end_speed()))
if path.is_type_print_segment(
): # If there is currently a segment being processed,
u_start = ratio * path.get_start_speed(
) # The end speed, depends on the angle to the next
else:
u_start = 0
if path.is_type_print_segment(
): # If there are paths in queue, we might not have to slow down
u_end = ratio * path.get_end_speed(
) # The start speed. Depends on the angle to the prev.
else:
u_end = 0
tm_start = (
Vm - u_start) / a # Calculate the time for when max speed is met.
tm_end = (Vm -
u_end) / a # Calculate the time for when max speed is met.
sm_start = min(
u_start * tm_start + 0.5 * a * tm_start**2,
s / 2.0) # Calculate the distance traveled when max speed is met
sm_end = min(
u_end * tm_end + 0.5 * a * tm_end**2,
s / 2.0) # Calculate the distance traveled when max speed is met
distances_start = np.arange(0, sm_start, ds) # Table of distances
distances_end = np.arange(0, sm_end, ds) # Table of distances
timestamps_start = (-u_start +
np.sqrt(2.0 * a * distances_start +
u_start**2)) / a # When ticks occur
timestamps_end = (-u_end + np.sqrt(2.0 * a * distances_end +
u_end**2)) / a # When ticks occur
delays_start = np.diff(
timestamps_start
) / 2.0 # We are more interested in the delays pr second.
delays_end = np.diff(
timestamps_end
) / 2.0 # We are more interested in the delays pr second.
#delays_start = np.array([delays_start, delays_start]).transpose().flatten()
#delays_end = np.array([delays_end, delays_end]).transpose().flatten()
i_steps = num_steps - len(delays_start) - len(
delays_end) # Find out how many delays are missing
i_delays = [(ds / Vm) / 2.0] * i_steps # Make the intermediate steps
delays = np.concatenate(
[delays_start, i_delays,
np.flipud(delays_end)]) # Add the missing delays.
td = num_steps / steps_pr_meter # Calculate the actual travelled distance
if vec < 0: # If the vector is negative, negate it.
td *= -1.0
# If the axes are X or Y, we need to transform back in case of
# H-belt or some other transform.
if axis == "X" or axis == "Y":
(td_x, td_y) = path.stepper_to_axis(td, axis)
self.current_pos["X"] += td_x
self.current_pos["Y"] += td_y
else:
self.current_pos[axis] += td # Update the global position vector
return (step_pins, dir_pins, option_pins, delays
) # return the pin states and the data
class PathPlanner:
''' Init the planner '''
def __init__(self, steppers, pru_firmware):
self.steppers = steppers
self.pru = Pru(pru_firmware) # Make the PRU
self.paths = Queue.Queue(10) # Make a queue of paths
self.current_pos = {"X":0.0, "Y":0.0, "Z":0.0, "E":0.0,"H":0.0}
self.running = True # Yes, we are running
self.pru_data = []
self.t = Thread(target=self._do_work) # Make the thread
self.t.daemon = True
self.travel_length = {"X":0.0, "Y":0.0, "Z":0.0}
self.center_offset = {"X":0.0, "Y":0.0, "Z":0.0}
if __name__ != '__main__':
self.t.start()
''' Set travel length (printer size) for all axis '''
def set_travel_length(self, travel):
self.travel_length = travel
''' Set offset of printer center for all axis '''
def set_center_offset(self, offset):
self.center_offset = offset
''' Set the acceleration used ''' # Fix me, move this to path
def set_acceleration(self, acceleration):
self.acceleration = acceleration
''' Home the given axis using endstops (min) '''
def home(self,axis):
if axis=="E" or axis=="H":
return
logging.debug("homing "+axis)
p = Path({axis:-self.travel_length[axis]}, 0.01, "RELATIVE", False, False)
p.set_homing_feedrate()
#p.set_max_speed(p.get_max_speed()*0.2)
self.add_path(p)
path = Path({axis:-self.center_offset[axis]}, 0.01, "G92")
self.add_path(path)
p = Path({axis:0}, 0.01, "ABSOLUTE")
p.set_homing_feedrate()
#p.set_max_speed(p.get_max_speed()*0.2)
self.add_path(p)
self.wait_until_done()
logging.debug("homing done for "+axis)
''' Add a path segment to the path planner '''
def add_path(self, new):
if not new.is_G92():
if hasattr(self, 'prev'):
self.prev.set_next(new)
new.set_prev(self.prev)
self.prev = new
self.paths.put(new)
''' Return the number of paths currently on queue '''
def nr_of_paths(self):
return self.paths.qsize()
''' Set position for an axis '''
def _set_pos(self, axis, val):
self.current_pos[axis] = val
def wait_until_done(self):
'''Wait until planner is done'''
self.paths.join()
self.pru.wait_until_done()
def _do_work(self):
""" This loop pops a path, sends it to the PRU and waits for an event """
while self.running:
self.do_work()
def do_work(self):
""" This is just a separate function so the test at the bottom will pass """
path = self.paths.get() # Get the last path added
path.set_global_pos(self.current_pos.copy()) # Set the global position of the printer
if path.is_G92(): # Only set the position of the axes
for axis, pos in path.get_pos().iteritems(): # Run through all the axes in the path
self._set_pos(axis, pos)
self.paths.task_done()
return
for axis in path.get_axes(): # Run through all the axes in the path
data = self._make_data(path, axis)
if len(data[0]) > 0:
if len(self.pru_data) == 0:
self.pru_data = zip(*data)
else:
#self._braid_data1(self.pru_data, zip(*data))
self.pru_data = self._braid_data(self.pru_data, zip(*data))
while len(self.pru_data) > 0:
data = self.pru_data[0:0x10000/8]
del self.pru_data[0:0x10000/8]
if len(self.pru_data) > 0:
logging.debug("Long path segment is cut. remaining: "+str(len(self.pru_data)))
while not self.pru.has_capacity_for(len(data)*8):
#logging.debug("Pru full")
time.sleep(0.5)
self.pru.add_data(zip(*data))
self.pru.commit_data() # Commit data to ddr
self.pru_data = []
self.paths.task_done()
path.unlink() # Remove reference to enable garbage collection
def emergency_interrupt(self):
self.pru.emergency_interrupt()
while True:
try:
path = self.paths.get(block=False)
if path != None:
self.paths.task_done()
path.unlink()
except Queue.Empty:
break
def _braid_data(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
return braid.braid_data_c(data1, data2) # Use the Optimized C-function foir this.
def _braid_data1(self, data1, data2):
""" Braid/merge together the data from the two data sets"""
line = 0
(pin1,dir1,o1, dly1) = data1[line]
(pin2,dir2,o2, dly2) = data2.pop(0)
while True:
dly = min(dly1, dly2)
dly1 -= dly
dly2 -= dly
try:
if dly1==0 and dly2==0:
data1[line] = (pin1|pin2, dir1 | dir2,o1 | o2, dly)
(pin1,dir1,o1, dly1) = data1[line+1]
(pin2,dir2,o2, dly2) = data2.pop(0)
elif dly1==0:
data1[line] = (pin1, dir1 ,o1 , dly)
(pin1,dir1,o1, dly1) = data1[line+1]
elif dly2==0:
data1.insert(line, (pin2, dir2, o2, dly))
(pin2,dir2,o2, dly2) = data2.pop(0)
line += 1
except IndexError:
break
if dly2 > 0:
#data1[line] = (data1[line][0],data1[line][1],data1[line][2], data1[line][3]+dly2)
data1.append((pin2, dir2,o2, dly2))
line += 1
elif dly1 > 0:
data1[line] = (data1[line][0], data1[line][1],data1[line][2], data1[line][3]+dly1)
#data1.pop(line+1)
while len(data2) > 0:
line += 1
(pin2,dir2,o2, dly2) = data2.pop(0)
data1.append((pin2, dir2,o2, dly2))
#while len(data1) > line+1:
# line += 1
# (pin1, dir1,o1, dly1) = data1[line]
# data1[line] = (pin2|pin1,dir1 | dir2,o1 | o2, dly1)
''' Join the thread '''
def exit(self):
self.running = False
self.pru.join()
self.t.join()
def force_exit(self):
self.running = False
self.pru.force_exit()
''' Make the data for the PRU or steppers '''
def _make_data(self, path, axis):
stepper = self.steppers[axis]
steps_pr_meter = stepper.get_steps_pr_meter()
vec = path.get_axis_length(axis) # Total travel distance
num_steps = int(abs(vec) * steps_pr_meter) # Number of steps to tick
if num_steps == 0:
return ([], [])
step_pin = stepper.get_step_pin() # Get the step pin
dir_pin = stepper.get_dir_pin() # Get the direction pin
#if stepper.get_direction() > 0:
dir_pin = 0 if vec < 0 else dir_pin # Disable the dir-pin if we are going backwards
#else:
# dir_pin = 0 if vec >= 0 else dir_pin
step_pins = [step_pin]*num_steps # Make the pin states
dir_pins = [dir_pin]*num_steps
option_pins = [1 if path.is_cancellable() else 0]*num_steps
s = abs(path.get_axis_length(axis)) # Get the length of the vector
ratio = path.get_axis_ratio(axis) # Ratio is the length of this axis to the total length
Vm = path.get_max_speed()*ratio # The travelling speed in m/s
a = self.acceleration*ratio # Accelleration in m/s/s
ds = 1.0/steps_pr_meter # Delta S, distance in meters travelled pr step.
#logging.debug('Start speed '+str(path.get_start_speed()))
#logging.debug('End speed '+str(path.get_end_speed()))
if path.is_type_print_segment(): # If there is currently a segment being processed,
u_start = ratio*path.get_start_speed() # The end speed, depends on the angle to the next
else:
u_start = 0
if path.is_type_print_segment(): # If there are paths in queue, we might not have to slow down
u_end = ratio*path.get_end_speed() # The start speed. Depends on the angle to the prev.
else:
u_end = 0
tm_start = (Vm-u_start)/a # Calculate the time for when max speed is met.
tm_end = (Vm-u_end)/a # Calculate the time for when max speed is met.
sm_start = min(u_start*tm_start + 0.5*a*tm_start**2, s/2.0) # Calculate the distance traveled when max speed is met
sm_end = min(u_end*tm_end + 0.5*a*tm_end**2, s/2.0) # Calculate the distance traveled when max speed is met
distances_start = np.arange(0, sm_start, ds) # Table of distances
distances_end = np.arange(0, sm_end, ds) # Table of distances
timestamps_start = (-u_start+np.sqrt(2.0*a*distances_start+u_start**2))/a # When ticks occur
timestamps_end = (-u_end +np.sqrt(2.0*a*distances_end+u_end**2))/a # When ticks occur
delays_start = np.diff(timestamps_start)/2.0 # We are more interested in the delays pr second.
delays_end = np.diff(timestamps_end)/2.0 # We are more interested in the delays pr second.
#delays_start = np.array([delays_start, delays_start]).transpose().flatten()
#delays_end = np.array([delays_end, delays_end]).transpose().flatten()
i_steps = num_steps-len(delays_start)-len(delays_end) # Find out how many delays are missing
i_delays = [(ds/Vm)/2.0]*i_steps # Make the intermediate steps
delays = np.concatenate([delays_start, i_delays, np.flipud(delays_end)])# Add the missing delays.
td = num_steps/steps_pr_meter # Calculate the actual travelled distance
if vec < 0: # If the vector is negative, negate it.
td *= -1.0
# If the axes are X or Y, we need to transform back in case of
# H-belt or some other transform.
if axis == "X" or axis == "Y":
(td_x, td_y) = path.stepper_to_axis(td, axis)
self.current_pos["X"] += td_x
self.current_pos["Y"] += td_y
else:
self.current_pos[axis] += td # Update the global position vector
return (step_pins,dir_pins,option_pins, delays) # return the pin states and the data
class Path_planner:
''' Init the planner '''
def __init__(self, steppers, current_pos):
self.steppers = steppers
self.pru = Pru() # Make the PRU
self.paths = Queue.Queue(30) # Make a queue of paths
self.current_pos = current_pos # Current position in (x, y, z, e)
self.running = True # Yes, we are running
self.pru_data = defaultdict(int)
self.t = Thread(target=self._do_work) # Make the thread
self.t.start()
''' Set the acceleration used '''
def set_acceleration(self, acceleration):
self.acceleration = acceleration
''' Add a path segment to the path planner '''
def add_path(self, new):
self.paths.put(new)
if hasattr(self, 'prev'):
self.prev.set_next(new)
new.set_prev(self.prev)
self.prev = new
''' Return the number of paths currently on queue '''
def nr_of_paths(self):
return self.paths.qsize()
''' Set position for an axis '''
def set_pos(self, axis, val):
self.current_pos[axis] = val
def wait_until_done(self):
'''Wait until planner is done'''
self.paths.join()
self.pru.wait_until_done()
''' This loop pops a path, sends it to the PRU
and waits for an event '''
def _do_work(self):
events_waiting = 0
while self.running:
try:
path = self.paths.get(timeout = 1) # Get the last path added
path.set_global_pos(self.current_pos.copy()) # Set the global position of the printer
axes_added = 0
all_data = {}
slowest = 0
for axis in path.get_axes(): # Run through all the axes in the path
stepper = self.steppers[axis] # Get a handle of the stepper
data = self._make_data(path, axis)
if len(data[0]) > 0:
all_data[axis] = data # Generate the timing and pin data
slowest = max(slowest, sum(data[1]))
for axis in all_data:
packet = all_data[axis]
delays = np.array(packet[1])
diff = (slowest-sum(delays))/len(delays)
for j, delay in enumerate(delays):
delays[j] = max(delay+diff, 1.0/10000.0) # min 0.2ms
data = (packet[0], delays)
if "Z" in all_data: # HACK! The Z-axis cannot be combined with the other data. Somehow it goes backwards...
packet = all_data["Z"]
while not self.pru.has_capacity_for(len(packet[0])*8):# Wait until the PRU has capacity for this chunk of data
time.sleep(1)
if self.pru.add_data(packet) > 0:
self.pru.commit_data()
del all_data["Z"]
for axis in all_data: # Commit the other axes
packet = all_data[axis]
z = zip(np.cumsum(packet[1]), packet[0])
for item in z:
self.pru_data[item[0]] += item[1]
if len(self.pru_data) > 0:
z = zip(*sorted(self.pru_data.items()))
self.pru_data = (list(z[1]), list(np.diff([0]+list(z[0]))))
while not self.pru.has_capacity_for(len(self.pru_data[0])*8):
time.sleep(0.1)
self.pru.add_data(self.pru_data)
self.pru.commit_data() # Commit data to ddr
self.pru_data = defaultdict(int)
self.paths.task_done()
except Queue.Empty:
pass
def merge_data(self):
self.pru_data
def _add_or_append(self, old, ts, pin):
if ts == old[-1][0]:
return (x, old[-1][1]+y)
return (x, y)
''' Join the thread '''
def exit(self):
self.running = False
self.pru.join()
logging.debug("pru joined")
self.t.join()
logging.debug("path planner joined")
''' Make the data for the PRU or steppers '''
def _make_data(self, path, axis):
stepper = self.steppers[axis]
steps_pr_meter = stepper.get_steps_pr_meter()
vec = path.get_axis_length(axis) # Total travel distance
num_steps = int(abs(vec) * steps_pr_meter) # Number of steps to tick
if num_steps == 0:
return ([], [])
step_pin = stepper.get_step_pin() # Get the step pin
dir_pin = stepper.get_dir_pin() # Get the direction pin
dir_pin = 0 if vec < 0 else dir_pin # Disable the dir-pin if we are going backwards
pins = [step_pin | dir_pin, dir_pin]*num_steps # Make the pin states
s = abs(path.get_axis_length(axis)) # Get the length of the vector
ratio = path.get_axis_ratio(axis) # Ratio is the length of this axis to the total length
Vm = path.get_max_speed()*ratio # The travelling speed in m/s
a = self.acceleration*ratio # Accelleration in m/s/s
ds = 1.0/steps_pr_meter # Delta S, distance in meters travelled pr step.
if self.pru.is_processing(): # If there is currently a segment being processed,
u_start = ratio*path.get_start_speed() # The end speed, depends on the angle to the next
else:
u_start = 0
if self.paths.qsize() > 0: # If there are paths in queue, we do not have to slow down
u_end = ratio*path.get_end_speed() # The start speed. Depends on the angle to the prev.
else:
u_end = 0
#print "Max speed for "+axis+" is "+str(Vm)
#print "Start speed for "+axis+" is "+str(u_start)
#print "End speed for "+axis+" is "+str(u_end)
tm_start = (Vm-u_start)/a # Calculate the time for when max speed is met.
tm_end = (Vm-u_end)/a # Calculate the time for when max speed is met.
sm_start = min(u_start*tm_start + 0.5*a*tm_start**2, s/2.0) # Calculate the distance traveled when max speed is met
sm_end = min(u_end*tm_end + 0.5*a*tm_end**2, s/2.0) # Calculate the distance traveled when max speed is met
distances_start = list(np.arange(0, sm_start, ds)) # Table of distances
distances_end = list(np.arange(0, sm_end, ds)) # Table of distances
timestamps_start = [(-u_start+np.sqrt(2.0*a*ss+u_start**2))/a for ss in distances_start]# When ticks occur
timestamps_end = [(-u_end +np.sqrt(2.0*a*ss+u_end**2))/a for ss in distances_end]# When ticks occur
delays_start = np.diff(timestamps_start)/2.0 # We are more interested in the delays pr second.
delays_end = np.diff(timestamps_end)/2.0 # We are more interested in the delays pr second.
delays_start = list(np.array([delays_start, delays_start]).transpose().flatten())
delays_end = list(np.array([delays_end, delays_end]).transpose().flatten())
i_steps = 2*num_steps-len(delays_start)-len(delays_end) # Find out how many delays are missing
i_delays = [(ds/Vm)/2.0]*i_steps # Make the intermediate steps
delays = delays_start+i_delays+delays_end[::-1] # Add the missing delays. These are max_speed
td = num_steps/steps_pr_meter # Calculate the actual travelled distance
if vec < 0: # If the vector is negative, negate it.
td *= -1.0
path.set_travelled_distance(axis, td) # Set the travelled distance back in the path
self.current_pos[axis] += td # Update the global position vector
#with open(axis+"_delays", "w+") as f:
# f.write(", ".join(map(str, delays)))
return (pins, delays) # return the pin states and the data