def __init__(self):
# params for driving
self.regular_speed = 0.3
self.speedrun_speed = 1.3
#unit_length = 0.195 #For regular runs
unit_length = 0.188 #For speed runs
# positions
self.pos = (0, 0)
self.target_list = [(7, 7), (7, 8), (8, 7), (8, 8)]
# circular buffer for nodes visited
self.len_nodes_visited = 5
self.nodes_visited = [0] * self.len_nodes_visited
self.pointer = 0
# initializing objects
self.MoveComputer = MoveComputer2()
self.graph = Graph2()
self.driver = DriveStep(self.pos, unit_length=unit_length)
self.planner = PathPlanner()
self.previous_graph = None # dictionary, not graph object.
self.save = False # for saving the graph
self.step = False # steps vs continuous drive
def demo_path_planner():
global pp
global initial_state
global start_state
global animation_delay_sec
start_time = time.time()
# Initialize to global variable
demo_initial_state = initial_state
# If global variable was not set, use a default value
if not is_valid_file(demo_initial_state):
demo_initial_state = "examples/initialState/Gorbett-start1.txt"
# Create initial state
start_state = pp.initializeStartState(demo_initial_state)
# NOTE: State is an object that contains a dictionary, state.blocks, in the form {blockid: blockObject}
# The blockObject should contain all the information necessary to visualize it
# Create a planner for individual moves
path_planner = PathPlanner()
# Display the initial state
# vis.show_state(start_state)
# Get moves to put block5 on the table in the given state
print("------------------------------")
print("planMove - block5 above table")
print("------------------------------")
new_state, moves, visited_states = path_planner.planMove(("block5", "above", "table"), start_state)
elapsed = str(timedelta(seconds=time.time() - start_time))
for move in moves:
print(move[0])
vis.show_state(move[1], move[0])
time.sleep(animation_delay_sec)
# Print final state w/o command. This is needed to 'remove' command formatting
vis.show_state(move[1])
print("Completed move planning in {0} moves after visiting {1} unique states in {2}".format(
str(len(moves)), str(visited_states), elapsed))
# Get moves to put slide block4 next to block6
print("--------------------------------------")
print("planMove - slide block4 next to block6")
print("--------------------------------------")
start_time = time.time()
new_state, moves, visited_states = path_planner.planMove(("block4", "next-to", "block6"), new_state)
elapsed = str(timedelta(seconds=time.time() - start_time))
for move in moves:
print(move[0])
vis.show_state(move[1], move[0])
time.sleep(animation_delay_sec)
# Print final state w/o command. This is needed to 'remove' command formatting
vis.show_state(move[1])
print("Completed move planning in {0} moves after visiting {1} unique states in {2}".format(
str(len(moves)), str(visited_states), elapsed))
def do_put(obj_spec, location):
# Put an object described by the given ObjectSpec, into a place satisfying Location
global pp
global initial_state
global start_state
start_time = time.time()
path_planner = PathPlanner()
# Prepare a move as a 3-element list:
# object to move
# relation or command
# target or object
move = [None, None, None]
# We're going to have to make this fancier, but for now, let's just
# pick any object that satisfies the given obj_spec, and do the same
# for the location.relativeTo.
possible_objects = find_objects(obj_spec)
if len(possible_objects) == 0:
print("I can't find any " + str(obj_spec) + ".")
return
move[0] = possible_objects[0].id
print("Chose", move[0], "from", possible_objects, "to satisfy", obj_spec)
if location.relation is None:
print("I don't know how to move to " + str(location) + ".")
return
move[1] = location.relation
possible_relations = find_objects(location.relativeTo)
if len(possible_relations) == 0:
print("I can't find any " + str(location.relativeTo) + ".")
return
for r in possible_relations:
if r.id != move[0]:
move[2] = r.id
break
print("Chose", move[2], "from", possible_relations, "to satisfy", location.relativeTo)
if move[2] is None:
print("I can't find target object")
return
return move
def run(points, obsticles, heights):
# Stores the path to DFAServer working directory
pathToApp = "C:\\Users\\Anders Fredriksen\\Desktop\\Schuul\\Auto2\\flask_project3\\"
# Read the templates content of the template file
f1 = open(pathToApp + "templates_DFA\\Curved_Rail_general.dfa", "r")
dataGeneral = f1.read()
#print("data from template GENERAL:", dataGeneral)
f2 = open(pathToApp + "templates_DFA\\Curved_Rail_line.dfa", "r")
dataLine = f2.read()
#print("data from template LINE:", dataLine)
f3 = open(pathToApp + "templates_DFA\\Curved_Rail_curve.dfa", "r")
dataCurve = f3.read()
#print("data from template CURVE:", dataCurve)
f4 = open(pathToApp + "templates_DFA\\Rail_beam.dfa", "r")
dataBeam = f4.read()
#print("data from template BEAM:", dataBeam)
x = 0
y = 0
z = 0
"""
# Intializing the points
pointA = (0, 0)
pointB = (0, 1000)
pointC = (1500, 4000)
# pointD = (10000, 10000)
pointD = (3500, -4000)
pointE = (10000, 6000)
pointF = (-1000, 5900)
pointG = (-2000, -2000)
pointH = (-7000, -1000)
pointI = (0, 8500)
pointJ = (11000, 9500)
pointK = (11500, -6000)
pointL = (2000, -7500)
pointM = (0, -500)
dataPoints = [pointA,
pointB, pointC, pointD, pointE, pointF, pointG, pointH, pointI, pointJ, pointK, pointL, pointM]
"""
dataPoints = points
heightpoints = heights
#print(heightpoints)
# Intializing the obstacles.
obstacle1 = (-1, 500, 100, 100, -70)
obstacles = [obstacle1]
pathPlanner = PathPlanner(2000)
path = pathPlanner.getPath(dataPoints)
# Now update the templates and generate the final result.
data = dataGeneral.replace("<GLOBAL_X>", str(x)) # data was initialized from dataGeneral...
data = data.replace("<GLOBAL_Y>", str(y))
data = data.replace("<GLOBAL_Z>", str(z))
# Insert path elements
pathElementsVarsTMP = "" # placeholder for path element vars
i = 0 # index of the path element
pathElements = ""
for p in path:
i += 1
if len(p) == 2:
# Make the line - line_rail_path_N
pathElements = pathElements + dataLine.replace("<ID>", str(i))
pathElements = pathElements.replace("<START_X>", str(p[0][0]))
pathElements = pathElements.replace("<START_Y>", str(p[0][1]))
pathElements = pathElements.replace("<START_Z>", str(z))
pathElements = pathElements.replace("<END_X>", str(p[1][0]))
pathElements = pathElements.replace("<END_Y>", str(p[1][1]))
pathElements = pathElements.replace("<END_Z>", str(z))
if i == 1:
pathElementsVarsTMP = "line_rail_path_1:,"
else:
pathElementsVarsTMP += "line_rail_path_" + str(i) + ":,"
else:
# Make the curve - curve_rail_path_N
pathElements = pathElements + dataCurve.replace("<ID>", str(i))
pathElements = pathElements.replace("<CURVE_RADIUS>", str(2000))
pathElements = pathElements.replace("<START_ANG>", str(p[0] / math.pi * 180))
pathElements = pathElements.replace("<END_ANG>", str(p[1] / math.pi * 180))
pathElements = pathElements.replace("<CENTER_X>", str(p[2][0]))
pathElements = pathElements.replace("<CENTER_Y>", str(p[2][1]))
pathElements = pathElements.replace("<CENTER_Z>", str(z))
pathElementsVarsTMP += "curve_rail_path_" + str(i) + ":,"
# Now insert the path elements to the common template
data = data.replace("<PATH_ELEMENTS>", pathElements)
# Insert the vars for path elements - by this time we know what are
# those - <PATH_ELEMENTS_VARS>
pathElementsVars = pathElementsVarsTMP
data = data.replace("<PATH_ELEMENTS_VARS>", pathElementsVars)
#print(data)
# Write the results to the common file
f = open(pathToApp + "A_Curved_Rail.dfa", "w")
f.write(data)
beams = ""
print(len(dataPoints))
for i in range(len(dataPoints)):
print(i)
beams = beams + dataBeam.replace("<ID>", str(i))
beams = beams.replace("<START_X>", str(dataPoints[i][0]))
beams = beams.replace("<START_Y>", str(dataPoints[i][1]))
print(dataPoints[i])
beams = beams.replace("<ROOF_HEIGHT>", str(1000*heightpoints[i]))#Må erstatte hardkoden med input-høyde fra brukeren
f.write(beams)
f.close()
def __init__(self):
firmware_version = "1.1.8~Raw Deal"
logging.info("Redeem initializing " + firmware_version)
printer = Printer()
self.printer = printer
Path.printer = printer
printer.firmware_version = firmware_version
# check for config files
if not os.path.exists("/etc/redeem/default.cfg"):
logging.error(
"/etc/redeem/default.cfg does not exist, this file is required for operation"
)
sys.exit() # maybe use something more graceful?
if not os.path.exists("/etc/redeem/local.cfg"):
logging.info("/etc/redeem/local.cfg does not exist, Creating one")
os.mknod("/etc/redeem/local.cfg")
# Parse the config files.
printer.config = CascadingConfigParser([
'/etc/redeem/default.cfg', '/etc/redeem/printer.cfg',
'/etc/redeem/local.cfg'
])
# Get the revision and loglevel from the Config file
level = self.printer.config.getint('System', 'loglevel')
if level > 0:
logging.getLogger().setLevel(level)
# Set up additional logging, if present:
if self.printer.config.getboolean('System', 'log_to_file'):
logfile = self.printer.config.get('System', 'logfile')
formatter = '%(asctime)s %(name)-12s %(levelname)-8s %(message)s'
printer.redeem_logging_handler = logging.handlers.RotatingFileHandler(
logfile, maxBytes=2 * 1024 * 1024)
printer.redeem_logging_handler.setFormatter(
logging.Formatter(formatter))
printer.redeem_logging_handler.setLevel(level)
logging.getLogger().addHandler(printer.redeem_logging_handler)
logging.info("-- Logfile configured --")
# Find out which capes are connected
self.printer.config.parse_capes()
self.revision = self.printer.config.replicape_revision
if self.revision:
logging.info("Found Replicape rev. " + self.revision)
else:
logging.warning("Oh no! No Replicape present!")
self.revision = "00B3"
# We set it to 5 axis by default
Path.NUM_AXES = 5
if self.printer.config.reach_revision:
logging.info("Found Reach rev. " +
self.printer.config.reach_revision)
if self.printer.config.reach_revision == "00A0":
Path.NUM_AXES = 8
elif self.printer.config.reach_revision == "00B0":
Path.NUM_AXES = 7
if self.revision in ["00A4", "0A4A", "00A3"]:
PWM.set_frequency(100)
elif self.revision in ["00B1", "00B2", "00B3"]:
PWM.set_frequency(1000)
# Test the alarm framework
Alarm.printer = self.printer
Alarm.executor = AlarmExecutor()
alarm = Alarm(Alarm.ALARM_TEST, "Alarm framework operational")
# Init the Watchdog timer
printer.watchdog = Watchdog()
# Enable PWM and steppers
printer.enable = Enable("P9_41")
printer.enable.set_disabled()
# Init the Paths
Path.axis_config = printer.config.getint('Geometry', 'axis_config')
# Init the end stops
EndStop.inputdev = self.printer.config.get("Endstops", "inputdev")
# Set up key listener
Key_pin.listener = Key_pin_listener(EndStop.inputdev)
for es in ["Z2", "Y2", "X2", "Z1", "Y1",
"X1"]: # Order matches end stop inversion mask in Firmware
pin = self.printer.config.get("Endstops", "pin_" + es)
keycode = self.printer.config.getint("Endstops", "keycode_" + es)
invert = self.printer.config.getboolean("Endstops", "invert_" + es)
self.printer.end_stops[es] = EndStop(printer, pin, keycode, es,
invert)
self.printer.end_stops[es].stops = self.printer.config.get(
'Endstops', 'end_stop_' + es + '_stops')
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
if self.revision == "00A3":
printer.steppers["X"] = Stepper_00A3("GPIO0_27", "GPIO1_29",
"GPIO2_4", 0, "X")
printer.steppers["Y"] = Stepper_00A3("GPIO1_12", "GPIO0_22",
"GPIO2_5", 1, "Y")
printer.steppers["Z"] = Stepper_00A3("GPIO0_23", "GPIO0_26",
"GPIO0_15", 2, "Z")
printer.steppers["E"] = Stepper_00A3("GPIO1_28", "GPIO1_15",
"GPIO2_1", 3, "E")
printer.steppers["H"] = Stepper_00A3("GPIO1_13", "GPIO1_14",
"GPIO2_3", 4, "H")
elif self.revision == "00B1":
printer.steppers["X"] = Stepper_00B1("GPIO0_27", "GPIO1_29",
"GPIO2_4", 11, 0, "X")
printer.steppers["Y"] = Stepper_00B1("GPIO1_12", "GPIO0_22",
"GPIO2_5", 12, 1, "Y")
printer.steppers["Z"] = Stepper_00B1("GPIO0_23", "GPIO0_26",
"GPIO0_15", 13, 2, "Z")
printer.steppers["E"] = Stepper_00B1("GPIO1_28", "GPIO1_15",
"GPIO2_1", 14, 3, "E")
printer.steppers["H"] = Stepper_00B1("GPIO1_13", "GPIO1_14",
"GPIO2_3", 15, 4, "H")
elif self.revision == "00B2":
printer.steppers["X"] = Stepper_00B2("GPIO0_27", "GPIO1_29",
"GPIO2_4", 11, 0, "X")
printer.steppers["Y"] = Stepper_00B2("GPIO1_12", "GPIO0_22",
"GPIO2_5", 12, 1, "Y")
printer.steppers["Z"] = Stepper_00B2("GPIO0_23", "GPIO0_26",
"GPIO0_15", 13, 2, "Z")
printer.steppers["E"] = Stepper_00B2("GPIO1_28", "GPIO1_15",
"GPIO2_1", 14, 3, "E")
printer.steppers["H"] = Stepper_00B2("GPIO1_13", "GPIO1_14",
"GPIO2_3", 15, 4, "H")
elif self.revision == "00B3":
printer.steppers["X"] = Stepper_00B3("GPIO0_27", "GPIO1_29", 90,
11, 0, "X")
printer.steppers["Y"] = Stepper_00B3("GPIO1_12", "GPIO0_22", 91,
12, 1, "Y")
printer.steppers["Z"] = Stepper_00B3("GPIO0_23", "GPIO0_26", 92,
13, 2, "Z")
printer.steppers["E"] = Stepper_00B3("GPIO1_28", "GPIO1_15", 93,
14, 3, "E")
printer.steppers["H"] = Stepper_00B3("GPIO1_13", "GPIO1_14", 94,
15, 4, "H")
elif self.revision in ["00A4", "0A4A"]:
printer.steppers["X"] = Stepper_00A4("GPIO0_27", "GPIO1_29",
"GPIO2_4", 0, 0, "X")
printer.steppers["Y"] = Stepper_00A4("GPIO1_12", "GPIO0_22",
"GPIO2_5", 1, 1, "Y")
printer.steppers["Z"] = Stepper_00A4("GPIO0_23", "GPIO0_26",
"GPIO0_15", 2, 2, "Z")
printer.steppers["E"] = Stepper_00A4("GPIO1_28", "GPIO1_15",
"GPIO2_1", 3, 3, "E")
printer.steppers["H"] = Stepper_00A4("GPIO1_13", "GPIO1_14",
"GPIO2_3", 4, 4, "H")
# Init Reach steppers, if present.
if printer.config.reach_revision == "00A0":
printer.steppers["A"] = Stepper_reach_00A4("GPIO2_2", "GPIO1_18",
"GPIO0_14", 5, 5, "A")
printer.steppers["B"] = Stepper_reach_00A4("GPIO1_16", "GPIO0_5",
"GPIO0_14", 6, 6, "B")
printer.steppers["C"] = Stepper_reach_00A4("GPIO0_3", "GPIO3_19",
"GPIO0_14", 7, 7, "C")
elif printer.config.reach_revision == "00B0":
printer.steppers["A"] = Stepper_reach_00B0("GPIO1_16", "GPIO0_5",
"GPIO0_3", 5, 5, "A")
printer.steppers["B"] = Stepper_reach_00B0("GPIO2_2", "GPIO0_14",
"GPIO0_3", 6, 6, "B")
# Enable the steppers and set the current, steps pr mm and
# microstepping
for name, stepper in self.printer.steppers.iteritems():
stepper.in_use = printer.config.getboolean('Steppers',
'in_use_' + name)
stepper.direction = printer.config.getint('Steppers',
'direction_' + name)
stepper.has_endstop = printer.config.getboolean(
'Endstops', 'has_' + name)
stepper.set_current_value(
printer.config.getfloat('Steppers', 'current_' + name))
stepper.set_steps_pr_mm(
printer.config.getfloat('Steppers', 'steps_pr_mm_' + name))
stepper.set_microstepping(
printer.config.getint('Steppers', 'microstepping_' + name))
stepper.set_decay(
printer.config.getint("Steppers", "slow_decay_" + name))
# Add soft end stops
Path.soft_min[Path.axis_to_index(name)] = printer.config.getfloat(
'Endstops', 'soft_end_stop_min_' + name)
Path.soft_max[Path.axis_to_index(name)] = printer.config.getfloat(
'Endstops', 'soft_end_stop_max_' + name)
slave = printer.config.get('Steppers', 'slave_' + name)
if slave:
Path.add_slave(name, slave)
logging.debug("Axis " + name + " has slave " + slave)
# Commit changes for the Steppers
#Stepper.commit()
Stepper.printer = printer
# Delta printer setup
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
opts = [
"Hez", "L", "r", "Ae", "Be", "Ce", "A_radial", "B_radial",
"C_radial", "A_tangential", "B_tangential", "C_tangential"
]
for opt in opts:
Delta.__dict__[opt] = printer.config.getfloat('Delta', opt)
Delta.recalculate()
# Discover and add all DS18B20 cold ends.
import glob
paths = glob.glob("/sys/bus/w1/devices/28-*/w1_slave")
logging.debug("Found cold ends: " + str(paths))
for i, path in enumerate(paths):
self.printer.cold_ends.append(ColdEnd(path, "ds18b20-" + str(i)))
logging.info("Found Cold end " + str(i) + " on " + path)
# Make Mosfets, thermistors and extruders
heaters = ["E", "H", "HBP"]
if self.printer.config.reach_revision:
heaters.extend(["A", "B", "C"])
for e in heaters:
# Mosfets
channel = self.printer.config.getint("Heaters", "mosfet_" + e)
self.printer.mosfets[e] = Mosfet(channel)
# Thermistors
adc = self.printer.config.get("Heaters", "path_adc_" + e)
chart = self.printer.config.get("Heaters", "temp_chart_" + e)
resistance = self.printer.config.getfloat("Heaters",
"resistance_" + e)
self.printer.thermistors[e] = Thermistor(adc, "MOSFET " + e, chart,
resistance)
self.printer.thermistors[e].printer = printer
# Extruders
onoff = self.printer.config.getboolean('Heaters', 'onoff_' + e)
prefix = self.printer.config.get('Heaters', 'prefix_' + e)
if e != "HBP":
self.printer.heaters[e] = Extruder(self.printer.steppers[e],
self.printer.thermistors[e],
self.printer.mosfets[e], e,
onoff)
else:
self.printer.heaters[e] = HBP(self.printer.thermistors[e],
self.printer.mosfets[e], onoff)
self.printer.heaters[e].prefix = prefix
self.printer.heaters[e].P = self.printer.config.getfloat(
'Heaters', 'pid_p_' + e)
self.printer.heaters[e].I = self.printer.config.getfloat(
'Heaters', 'pid_i_' + e)
self.printer.heaters[e].D = self.printer.config.getfloat(
'Heaters', 'pid_d_' + e)
# Min/max settings
self.printer.heaters[e].min_temp = self.printer.config.getfloat(
'Heaters', 'min_temp_' + e)
self.printer.heaters[e].max_temp = self.printer.config.getfloat(
'Heaters', 'max_temp_' + e)
self.printer.heaters[
e].max_temp_rise = self.printer.config.getfloat(
'Heaters', 'max_rise_temp_' + e)
self.printer.heaters[
e].max_temp_fall = self.printer.config.getfloat(
'Heaters', 'max_fall_temp_' + e)
# Init the three fans. Argument is PWM channel number
self.printer.fans = []
if self.revision == "00A3":
self.printer.fans.append(Fan(0))
self.printer.fans.append(Fan(1))
self.printer.fans.append(Fan(2))
elif self.revision == "0A4A":
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
elif self.revision in ["00B1", "00B2", "00B3"]:
self.printer.fans.append(Fan(7))
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
if printer.config.reach_revision == "00A0":
self.printer.fans.append(Fan(14))
self.printer.fans.append(Fan(15))
self.printer.fans.append(Fan(7))
# Disable all fans
for f in self.printer.fans:
f.set_value(0)
# Init the servos
printer.servos = []
servo_nr = 0
while (printer.config.has_option("Servos", "servo_" + str(servo_nr) +
"_enable")):
if printer.config.getboolean("Servos",
"servo_" + str(servo_nr) + "_enable"):
channel = printer.config.get(
"Servos", "servo_" + str(servo_nr) + "_channel")
pulse_min = printer.config.getfloat(
"Servos", "servo_" + str(servo_nr) + "_pulse_min")
pulse_max = printer.config.getfloat(
"Servos", "servo_" + str(servo_nr) + "_pulse_max")
angle_min = printer.config.getfloat(
"Servos", "servo_" + str(servo_nr) + "_angle_min")
angle_max = printer.config.getfloat(
"Servos", "servo_" + str(servo_nr) + "_angle_max")
angle_init = printer.config.getfloat(
"Servos", "servo_" + str(servo_nr) + "_angle_init")
s = Servo(channel, pulse_min, pulse_max, angle_min, angle_max,
angle_init)
printer.servos.append(s)
logging.info("Added servo " + str(servo_nr))
servo_nr += 1
# Connect thermitors to fans
for t, therm in self.printer.heaters.iteritems():
for f, fan in enumerate(self.printer.fans):
if not self.printer.config.has_option(
'Cold-ends', "connect-therm-{}-fan-{}".format(t, f)):
continue
if printer.config.getboolean(
'Cold-ends', "connect-therm-{}-fan-{}".format(t, f)):
c = Cooler(therm, fan, "Cooler-{}-{}".format(t, f),
True) # Use ON/OFF on these.
c.ok_range = 4
opt_temp = "therm-{}-fan-{}-target_temp".format(t, f)
if printer.config.has_option('Cold-ends', opt_temp):
target_temp = printer.config.getfloat(
'Cold-ends', opt_temp)
else:
target_temp = 60
c.set_target_temperature(target_temp)
c.enable()
printer.coolers.append(c)
logging.info("Cooler connects therm {} with fan {}".format(
t, f))
# Connect fans to M106
printer.controlled_fans = []
for i, fan in enumerate(self.printer.fans):
if not self.printer.config.has_option(
'Cold-ends', "add-fan-{}-to-M106".format(i)):
continue
if self.printer.config.getboolean('Cold-ends',
"add-fan-{}-to-M106".format(i)):
printer.controlled_fans.append(self.printer.fans[i])
logging.info("Added fan {} to M106/M107".format(i))
# Connect the colds to fans
for ce, cold_end in enumerate(self.printer.cold_ends):
for f, fan in enumerate(self.printer.fans):
option = "connect-ds18b20-{}-fan-{}".format(ce, f)
if self.printer.config.has_option('Cold-ends', option):
if self.printer.config.getboolean('Cold-ends', option):
c = Cooler(cold_end, fan,
"Cooler-ds18b20-{}-{}".format(ce, f), False)
c.ok_range = 4
opt_temp = "cooler_{}_target_temp".format(ce)
if printer.config.has_option('Cold-ends', opt_temp):
target_temp = printer.config.getfloat(
'Cold-ends', opt_temp)
else:
target_temp = 60
c.set_target_temperature(target_temp)
c.enable()
printer.coolers.append(c)
logging.info(
"Cooler connects temp sensor ds18b20 {} with fan {}"
.format(ce, f))
# Init roatray encs.
printer.filament_sensors = []
# Init rotary encoders
printer.rotary_encoders = []
for ex in ["E", "H", "A", "B", "C"]:
if not printer.config.has_option('Rotary-encoders',
"enable-{}".format(ex)):
continue
if printer.config.getboolean("Rotary-encoders",
"enable-{}".format(ex)):
logging.debug("Rotary encoder {} enabled".format(ex))
event = printer.config.get("Rotary-encoders",
"event-{}".format(ex))
cpr = printer.config.getint("Rotary-encoders",
"cpr-{}".format(ex))
diameter = printer.config.getfloat("Rotary-encoders",
"diameter-{}".format(ex))
r = RotaryEncoder(event, cpr, diameter)
printer.rotary_encoders.append(r)
# Append as Filament Sensor
ext_nr = Path.axis_to_index(ex) - 3
sensor = FilamentSensor(ex, r, ext_nr, printer)
alarm_level = printer.config.getfloat(
"Filament-sensors", "alarm-level-{}".format(ex))
logging.debug("Alarm level" + str(alarm_level))
sensor.alarm_level = alarm_level
printer.filament_sensors.append(sensor)
# Make a queue of commands
self.printer.commands = JoinableQueue(10)
# Make a queue of commands that should not be buffered
self.printer.sync_commands = JoinableQueue()
self.printer.unbuffered_commands = JoinableQueue(10)
# Bed compensation matrix
Path.matrix_bed_comp = printer.load_bed_compensation_matrix()
Path.matrix_bed_comp_inv = np.linalg.inv(Path.matrix_bed_comp)
logging.debug("Loaded bed compensation matrix: \n" +
str(Path.matrix_bed_comp))
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
Path.max_speeds[i] = printer.config.getfloat(
'Planner', 'max_speed_' + axis.lower())
Path.min_speeds[i] = printer.config.getfloat(
'Planner', 'min_speed_' + axis.lower())
Path.jerks[i] = printer.config.getfloat('Planner',
'max_jerk_' + axis.lower())
Path.home_speed[i] = printer.config.getfloat(
'Homing', 'home_speed_' + axis.lower())
Path.home_backoff_speed[i] = printer.config.getfloat(
'Homing', 'home_backoff_speed_' + axis.lower())
Path.home_backoff_offset[i] = printer.config.getfloat(
'Homing', 'home_backoff_offset_' + axis.lower())
Path.steps_pr_meter[i] = printer.steppers[axis].get_steps_pr_meter(
)
Path.backlash_compensation[i] = printer.config.getfloat(
'Steppers', 'backlash_' + axis.lower())
dirname = os.path.dirname(os.path.realpath(__file__))
# Create the firmware compiler
pru_firmware = PruFirmware(dirname + "/firmware/firmware_runtime.p",
dirname + "/firmware/firmware_runtime.bin",
dirname + "/firmware/firmware_endstops.p",
dirname + "/firmware/firmware_endstops.bin",
self.printer, "/usr/bin/pasm")
printer.move_cache_size = printer.config.getfloat(
'Planner', 'move_cache_size')
printer.print_move_buffer_wait = printer.config.getfloat(
'Planner', 'print_move_buffer_wait')
printer.min_buffered_move_time = printer.config.getfloat(
'Planner', 'min_buffered_move_time')
printer.max_buffered_move_time = printer.config.getfloat(
'Planner', 'max_buffered_move_time')
self.printer.processor = GCodeProcessor(self.printer)
self.printer.plugins = PluginsController(self.printer)
# Path planner
travel_default = False
center_default = False
home_default = False
# Setting acceleration before PathPlanner init
for axis in printer.steppers.keys():
Path.acceleration[Path.axis_to_index(
axis)] = printer.config.getfloat(
'Planner', 'acceleration_' + axis.lower())
self.printer.path_planner = PathPlanner(self.printer, pru_firmware)
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
# Sometimes soft_end_stop aren't defined to be at the exact hardware boundary.
# Adding 100mm for searching buffer.
if printer.config.has_option('Geometry', 'travel_' + axis.lower()):
printer.path_planner.travel_length[
axis] = printer.config.getfloat('Geometry',
'travel_' + axis.lower())
else:
printer.path_planner.travel_length[axis] = (
Path.soft_max[i] - Path.soft_min[i]) + .1
if axis in ['X', 'Y', 'Z']:
travel_default = True
if printer.config.has_option('Geometry', 'offset_' + axis.lower()):
printer.path_planner.center_offset[
axis] = printer.config.getfloat('Geometry',
'offset_' + axis.lower())
else:
printer.path_planner.center_offset[axis] = (
Path.soft_min[i]
if Path.home_speed[i] > 0 else Path.soft_max[i])
if axis in ['X', 'Y', 'Z']:
center_default = True
if printer.config.has_option('Homing', 'home_' + axis.lower()):
printer.path_planner.home_pos[axis] = printer.config.getfloat(
'Homing', 'home_' + axis.lower())
else:
printer.path_planner.home_pos[
axis] = printer.path_planner.center_offset[axis]
if axis in ['X', 'Y', 'Z']:
home_default = True
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
if travel_default:
logging.warning(
"Axis travel (travel_*) set by soft limits, manual setup is recommended for a delta"
)
if center_default:
logging.warning(
"Axis offsets (offset_*) set by soft limits, manual setup is recommended for a delta"
)
if home_default:
logging.warning(
"Home position (home_*) set by soft limits or offset_*")
logging.info("Home position will be recalculated...")
# convert home_pos to effector space
Az = printer.path_planner.home_pos['X']
Bz = printer.path_planner.home_pos['Y']
Cz = printer.path_planner.home_pos['Z']
z_offset = Delta.vertical_offset(Az, Bz, Cz) # vertical offset
xyz = Delta.forward_kinematics2(Az, Bz,
Cz) # effector position
# The default home_pos, provided above, is based on effector space
# coordinates for carriage positions. We need to transform these to
# get where the effector actually is.
xyz[2] += z_offset
for i, a in enumerate(['X', 'Y', 'Z']):
printer.path_planner.home_pos[a] = xyz[i]
logging.info("Home position = %s" %
str(printer.path_planner.home_pos))
# Enable Stepper timeout
timeout = printer.config.getint('Steppers', 'timeout_seconds')
printer.swd = StepperWatchdog(printer, timeout)
if printer.config.getboolean('Steppers', 'use_timeout'):
printer.swd.start()
# Set up communication channels
printer.comms["USB"] = USB(self.printer)
printer.comms["Eth"] = Ethernet(self.printer)
if Pipe.check_tty0tty() or Pipe.check_socat():
printer.comms["octoprint"] = Pipe(printer, "octoprint")
printer.comms["toggle"] = Pipe(printer, "toggle")
printer.comms["testing"] = Pipe(printer, "testing")
printer.comms["testing_noret"] = Pipe(printer, "testing_noret")
# Does not send "ok"
printer.comms["testing_noret"].send_response = False
else:
logging.warning(
"Neither tty0tty or socat is installed! No virtual tty pipes enabled"
)
if __name__ == '__main__':
print('Graph based global trajectory planner')
env = Environment(input_file=INPUT_TREE_LOCATIONS,
x_range=X_RANGE + [OFFSET, -OFFSET],
y_range=Y_RANGE + [OFFSET, -OFFSET],
r_range=R_RANGE)
env.show()
env.view()
env.save_png(OUTPUT_GEN_DIR + 'tree_map.png')
# exit(1)
# Path planning
st = time.time()
planner = PathPlanner(env.Trees[:, :2])
way_points = planner.planning()
et = time.time() - st
print('Elapsed time:', et)
planner.save_graphs(OUTPUT_GEN_DIR)
# make images
save_images()
print('Path optimization')
Q = []
for k in range(len(way_points)):
x_k = way_points[k][0]
def heardEnter():
i,o,e = select.select([sys.stdin],[],[],0.0001)
for s in i:
if s == sys.stdin:
input = sys.stdin.readline()
return True
return False
####################################MAIN METHOD##########################################
colorseg = ColorSegStereo(colorseg_debug)
botcontrol = RobotControl()
pathplan = PathPlanner()
sensorcontrol = SensorControl()
processQ = Queue.Queue(2)
wrapper = Wrapper()
#client = Client()
tw = ThreadWrapper()
#nw = NetworkWrapper()
#sw = ServerWrapper()
def signal_handler(signal,frame):
print 'Stopping robot'
botcontrol.stop_robot()
#tw.stop()
if androidFlag:
nw.stop()
sys.exit(0)
y_range=Y_RANGE + [OFFSET, -OFFSET],
r_range=R_RANGE)
fig = plt.figure(num=1, figsize=(5, 5), dpi=100)
plt.rcParams["font.family"] = "Times New Roman"
plt.rcParams["xtick.direction"] = 'in'
plt.rcParams["ytick.direction"] = 'in'
ax = plt.axes()
ax.set_xlim(X_RANGE[0], X_RANGE[1])
ax.set_ylim(Y_RANGE[0], Y_RANGE[1])
ax.set_aspect('equal')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.grid(False)
planner = PathPlanner(env.Trees[:, 1:3])
way_points = np.array(planner.planning())
print(way_points)
fig = plt.figure(num=1, figsize=(5, 5), dpi=100)
plt.rcParams["font.family"] = "Times New Roman"
plt.rcParams["xtick.direction"] = 'in'
plt.rcParams["ytick.direction"] = 'in'
ax = plt.axes()
ax.set_xlim(X_RANGE[0], X_RANGE[1])
ax.set_ylim(Y_RANGE[0], Y_RANGE[1])
ax.set_aspect('equal')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
ax.grid(False)
class Controller(object):
def __init__(self):
# params for driving
self.regular_speed = 0.3
self.speedrun_speed = 1.3
#unit_length = 0.195 #For regular runs
unit_length = 0.188 #For speed runs
# positions
self.pos = (0, 0)
self.target_list = [(7, 7), (7, 8), (8, 7), (8, 8)]
# circular buffer for nodes visited
self.len_nodes_visited = 5
self.nodes_visited = [0] * self.len_nodes_visited
self.pointer = 0
# initializing objects
self.MoveComputer = MoveComputer2()
self.graph = Graph2()
self.driver = DriveStep(self.pos, unit_length=unit_length)
self.planner = PathPlanner()
self.previous_graph = None # dictionary, not graph object.
self.save = False # for saving the graph
self.step = False # steps vs continuous drive
def run_advanced(self):
""" blocking code while loop for mousebot reach center """
walls = self.driver.return_walls(
first=True) # compute first walls before movement
print('walls returned first: ', walls)
pos = self.pos
while (pos not in self.target_list) and not rospy.is_shutdown():
print(
'movement!======================================================================='
)
self.graph.update_graph(pos, walls)
next_pos = self.MoveComputer.compute_next_move(self.graph, pos)
print("Moving to: ", next_pos)
walls = self.driver.drive_advanced(
next_pos, self.regular_speed) # updates walls, position
pos = next_pos
# self.nodes_visited[self.pointer] = pos
# counter = 0
# for visited in self.nodes_visited:
# if visited == self.nodes_visited[self.pointer]:
# counter += 1
# if counter >=3:
# print("you're in a back and forth loop, altering MoveComputer2 code")
# self.pointer = (self.pointer+1)%self.len_nodes_visited
print(' ')
if self.step:
time.sleep(.3)
# do some final updates
self.pos = pos
self.graph.update_graph(pos,
walls) # final graph update with destination.
print("reached center")
def run(self):
""" blocking code while loop for mousebot reach center """
walls = self.driver.return_walls(
first=True) # compute first walls before movement
print('walls returned first: ', walls)
pos = self.pos
while (pos not in self.target_list) and not rospy.is_shutdown():
print(
'movement!======================================================================='
)
self.graph.update_graph(pos, walls)
next_pos = self.MoveComputer.compute_next_move(self.graph, pos)
print("Moving to: ", next_pos)
walls = self.driver.drive(
next_pos, self.regular_speed) # updates walls, position
pos = next_pos
self.nodes_visited[self.pointer] = pos
counter = 0
for visited in self.nodes_visited:
if visited == self.nodes_visited[self.pointer]:
counter += 1
if counter >= 3:
print(
"you're in a back and forth loop, altering MoveComputer2 code"
)
self.pointer = (self.pointer + 1) % self.len_nodes_visited
print(' ')
if self.step:
time.sleep(.3)
# do some final updates
self.pos = pos
self.graph.update_graph(pos,
walls) # final graph update with destination.
print("reached center")
# save dictionary
if self.save:
time.sleep(5)
print("should be complete graph, saving ... ", self.graph.graph)
np.save('mazes/graph.npy', self.graph.graph)
def test_speedrun(self):
target = (8, 8)
pos = self.pos
self.previous_graph = np.load('mazes/graph.npy',
allow_pickle='TRUE').item()
path_to_center = self.planner.a_star(self.previous_graph,
start=pos,
target=target)
print("path to center: ", path_to_center)
optimized_path = self.planner.consolidate_path(path_to_center)
print("optimized path: ", optimized_path)
while not rospy.is_shutdown():
for position in optimized_path:
print(
'movement!======================================================================='
)
print("Moving to: ", position)
self.driver.drive_speedrun(
position, self.speedrun_speed) # updates walls, position
pos = position
print(' ')
if self.step:
time.sleep(.3)
break
print("At Center.")
self.pos = pos
def test_pathplanning(self):
"""test pathplanning code with imported maze graph"""
target = (8, 8)
pos = self.pos
self.previous_graph = np.load('mazes/graph.npy',
allow_pickle='TRUE').item()
print("prev graph: ", self.previous_graph)
path_to_center = self.planner.a_star(self.previous_graph,
start=pos,
target=target)
while not rospy.is_shutdown():
for position in path_to_center:
print(
'movement!======================================================================='
)
print("Moving to: ", position)
walls = self.driver.drive(
position, self.regular_speed) # updates walls, position
pos = position
print(' ')
if self.step:
time.sleep(.3)
break
print("At Center.")
def run_with_astar(self, target):
# code for mousebot to reverse track back to starting point
pos = self.pos
path_to_home = self.planner.a_star(self.graph.graph,
start=pos,
target=target)
print(path_to_home)
while not rospy.is_shutdown():
for position in path_to_home:
print(
'movement!======================================================================='
)
print("Moving to: ", position)
walls = self.driver.drive(
position, self.regular_speed) # updates walls, position
pos = position
print(' ')
if self.step:
time.sleep(.3)
break
self.pos = pos
print("Back home.")
def speedrun(self, target):
pos = self.pos
self.previous_graph = np.load('mazes/graph.npy',
allow_pickle='TRUE').item()
path_to_center = self.planner.a_star(self.previous_graph,
start=pos,
target=target)
print("path to center: ", path_to_center)
optimized_path = self.planner.consolidate_path(path_to_center)
print("optimized path: ", optimized_path)
while not rospy.is_shutdown():
for position in optimized_path:
print(
'movement!======================================================================='
)
print("Moving to: ", position)
self.driver.drive_speedrun(
position, self.speedrun_speed) # updates walls, position
pos = position
print(' ')
if self.step:
time.sleep(.3)
break
print("At target.")
def call_back(data):
print('num input trees:', len(data.poses))
tree_positions = [[pose.position.x, pose.position.y]
for pose in data.poses]
tree_positions = np.array(tree_positions)
kd_tree = KDTree(tree_positions, leafsize=3)
# create planner and waypoints
planner = PathPlanner(tree_positions)
way_points = planner.planning()
print('way points ({}) : {}'.format(type(way_points), len(way_points)))
Q = configuration_sampling(way_points, NH)
print('|Q|=', len(Q))
print('Make pose graph ==========')
set_kh_tup = list(itertools.product(
range(len(way_points)), range(NH))) # create touple (k,h)
GQ = nx.DiGraph()
# add nodes
for k, h in set_kh_tup:
GQ.add_node(h + k*NH)
# add edges
for k, h in set_kh_tup:
if k >= len(way_points)-1:
break
for i in range(NH):
qi = Q[h + k*NH] # current way points
qj = Q[i + (k+1)*NH] # next way points
if LOCAL_PATH_PLANNER == 'DubinsPath': # Dubins Path Algorithm
px, py, pyaw, mode, length = DubinsPath.dubins_path_planning(
qi[0], qi[1], qi[2], # source pose
qj[0], qj[1], qj[2], # target pose
c=MIN_CURVATURE,
step_size=DUBINS_STEP)
# minimum distance to obstacle
min_d = float('inf')
for p in zip(px,py):
d,i = kd_tree.query(p, k=1)
if d < min_d:
min_d = d
# set edge weight
GQ.add_edge(h+k*NH,
i+(k+1)*NH,
weight=min_d)
# print(' Num edges: ', GQ.number_of_edges())
# add weighted edges
# for k in range(len(way_points)-1):
# for h in range(NH):
# for i in range(NH):
# qi = Q[h + k*NH]
# qj = Q[i + (k+1)*NH]
# # Dubins Path Algorithm
# if LOCAL_PATH_PLANNER == 'DubinsPath':
# px, py, pyaw, mode, length = DubinsPath.dubins_path_planning(
# qi[0], qi[1], qi[2],
# qj[0], qj[1], qj[2],
# c=CURVATURE)
# # check collision
# # if checkCollision(env,px,py,1.4):
# # continue
# # set edge weight
# GQ.add_edge(h+k*NH, i+(k+1)*NH, weight=length)
print(' Gq : |V|=', GQ.number_of_nodes(), ' |E|=', GQ.number_of_edges())
# add initial q as node at last of list
GQ.add_node(GQ.number_of_nodes())
for h in range(NH):
# Dubins Path Algorithm
if LOCAL_PATH_PLANNER == 'DubinsPath':
px, py, pyaw, mode, length = DubinsPath.dubins_path_planning(
q_init[0], q_init[1], q_init[2],
Q[h][0], Q[h][1], Q[h][2],
c=MIN_CURVATURE,
step_size=DUBINS_STEP)
# check collision
# if checkCollision(env,px,py,1.4):
# continue
GQ.add_edge(GQ.number_of_nodes()-1,
h,
weight=length)
GQ.add_node(GQ.number_of_nodes())
for h in range(NH):
# print(h + (len(way_points) - 1)*nh)
GQ.add_edge(h + (len(way_points)-1)*NH,
GQ.number_of_nodes()-1,
weight=0.0)
try:
path = nx.shortest_path(GQ,
source=GQ.number_of_nodes()-2,
target=GQ.number_of_nodes()-1,
weight='weight')
except:
print('cannot find shortest path')
return
print('found shortest path')
print('->', path)
path = list(path)
path.pop(-1)
# add initial q and node
# GQ.add_node(GQ.number_of_nodes())
# for h in range(NH):
# # Dubins Path Algorithm
# if LOCAL_PATH_PLANNER == 'DubinsPath':
# px, py, pyaw, mode, length = DubinsPath.dubins_path_planning(
# q_init[0], q_init[1], q_init[2],
# Q[h][0], Q[h][1], Q[h][2],
# c=CURVATURE)
# # check collision
# # if checkCollision(env,px,py,1.4):
# # continue
# GQ.add_edge(GQ.number_of_nodes()-1,
# h,
# weight=length)
# add dummy last node
# GQ.add_node(GQ.number_of_nodes())
# for h in range(NH):
# # print(h + (len(way_points) - 1)*nh)
# GQ.add_edge(h + (len(way_points)-1)*NH,
# GQ.number_of_nodes()-1,
# weight=0.0)
# print('C-Space pose graph |V|:', GQ.number_of_nodes())
# find shortest path
# try:
# path = nx.shortest_path(GQ,
# source=GQ.number_of_nodes()-2,
# target=GQ.number_of_nodes()-1,
# weight='weight')
# except:
# print('cannot find shortest path')
# sys.exit(-1)
# print('found shortest path')
# print('->', path)
# path = list(path)
# path.pop(-1)
traj_node2node = []
traj = []
yaws = []
total_length = 0.0
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = 'map'
for n, i in enumerate(path):
if n == len(path)-1:
break
if i == GQ.number_of_nodes()-2:
qi = q_init
else:
qi = Q[i]
qj = Q[path[n+1]]
if LOCAL_PATH_PLANNER == 'DubinsPath':
px, py, pyaw, mode, length = DubinsPath.dubins_path_planning(
qi[0], qi[1], qi[2],
qj[0], qj[1], qj[2],
c=MIN_CURVATURE,
step_size=DUBINS_STEP)
# tmp_list = [(xi, yi) for xi, yi in zip(px, py)]
# traj_node2node.append(tmp_list)
# traj = traj + tmp_list
# yaws = yaws + pyaw # pyaw 1-D array
# yaws.append(pyaw)
total_length += length
for (xi, yi, thi) in zip(px, py, pyaw):
_pose = PoseStamped()
_pose.header.stamp = path_msg.header.stamp
_pose.header.frame_id = path_msg.header.frame_id
_pose.pose.position.x = xi
_pose.pose.position.y = yi
_pose.pose.position.z = 0.0
q = quaternion_from_euler(0, 0, thi)
_pose.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
path_msg.poses.append(_pose)
print(len(path_msg.poses))
rospy.loginfo('creator motion plan!!')
make_marker_array_msg(way_points)
pub_path.publish(path_msg)
def __init__(self):
""" Init """
logging.info("Redeem initializing " + version)
self.printer = Printer()
# Parse the config files.
self.printer.config = CascadingConfigParser([
'/etc/redeem/default.cfg', '/etc/redeem/printer.cfg',
'/etc/redeem/local.cfg'
])
# Get the revision and loglevel from the Config file
self.revision = self.printer.config.get('System', 'revision', "A4")
level = self.printer.config.getint('System', 'loglevel')
if level > 0:
logging.getLogger().setLevel(level)
logging.info("Replicape revision " + self.revision)
# Init the end stops
EndStop.callback = self.end_stop_hit
EndStop.inputdev = self.printer.config.get("Endstops", "inputdev")
if self.revision == "A4" or self.revision == "A4A":
self.printer.end_stops["X1"] = EndStop(
"GPIO3_21", 112, "X1",
self.printer.config.getboolean("Endstops", "invert_X1"))
self.printer.end_stops["X2"] = EndStop(
"GPIO0_30", 113, "X2",
self.printer.config.getboolean("Endstops", "invert_X2"))
self.printer.end_stops["Y1"] = EndStop(
"GPIO1_17", 114, "Y1",
self.printer.config.getboolean("Endstops", "invert_Y1"))
self.printer.end_stops["Y2"] = EndStop(
"GPIO1_19", 115, "Y2",
self.printer.config.getboolean("Endstops", "invert_Y2"))
self.printer.end_stops["Z1"] = EndStop(
"GPIO0_31", 123, "Z1",
self.printer.config.getboolean("Endstops", "invert_Z1"))
self.printer.end_stops["Z2"] = EndStop(
"GPIO0_4", 117, "Z2",
self.printer.config.getboolean("Endstops", "invert_Z2"))
else:
self.printer.end_stops["X1"] = EndStop(
"GPIO0_14", 112, "X1",
self.printer.config.getboolean("Endstops", "invert_X1"))
self.printer.end_stops["X2"] = EndStop(
"GPIO3_21", 113, "X2",
self.printer.config.getboolean("Endstops", "invert_X2"))
self.printer.end_stops["Y1"] = EndStop(
"GPIO2_2", 114, "Y1",
self.printer.config.getboolean("Endstops", "invert_Y1"))
self.printer.end_stops["Y2"] = EndStop(
"GPIO0_31", 115, "Y2",
self.printer.config.getboolean("Endstops", "invert_Y2"))
self.printer.end_stops["Z1"] = EndStop(
"GPIO0_30", 123, "Z1",
self.printer.config.getboolean("Endstops", "invert_Z1"))
self.printer.end_stops["Z2"] = EndStop(
"GPIO0_4", 117, "Z2",
self.printer.config.getboolean("Endstops", "invert_Z2"))
if self.revision == "A3":
Stepper.revision = "A3"
Stepper.ENABLED = 6
Stepper.SLEEP = 5
Stepper.RESET = 4
Stepper.DECAY = 0
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
self.printer.steppers["X"] = Stepper("GPIO0_27", "GPIO1_29", "GPIO2_4",
0, "X",
self.printer.end_stops["X1"], 0,
0)
self.printer.steppers["Y"] = Stepper("GPIO1_12", "GPIO0_22", "GPIO2_5",
1, "Y",
self.printer.end_stops["Y1"], 1,
1)
self.printer.steppers["Z"] = Stepper("GPIO0_23", "GPIO0_26",
"GPIO0_15", 2, "Z",
self.printer.end_stops["Z1"], 2,
2)
self.printer.steppers["E"] = Stepper("GPIO1_28", "GPIO1_15", "GPIO2_1",
3, "E", None, 3, 3)
self.printer.steppers["H"] = Stepper("GPIO1_13", "GPIO1_14", "GPIO2_3",
4, "H", None, 4, 4)
# Enable the steppers and set the current, steps pr mm and
# microstepping
for name, stepper in self.printer.steppers.iteritems():
stepper.set_current_value(
self.printer.config.getfloat('Steppers', 'current_' + name))
stepper.in_use = self.printer.config.getboolean(
'Steppers', 'in_use_' + name)
stepper.set_steps_pr_mm(
self.printer.config.getfloat('Steppers',
'steps_pr_mm_' + name))
stepper.set_microstepping(
self.printer.config.getint('Steppers',
'microstepping_' + name))
stepper.direction = self.printer.config.getint(
'Steppers', 'direction_' + name)
stepper.set_decay(
self.printer.config.getboolean("Steppers",
"slow_decay_" + name))
stepper.has_endstop = self.printer.config.getboolean(
'Endstops', 'has_' + name)
# Commit changes for the Steppers
Stepper.commit()
# Find the path of the thermistors
path = "/sys/bus/iio/devices/iio:device0/in_voltage"
# init the 3 thermistors
therm_ext1 = Thermistor(
path + "4_raw", "MOSFET Ext 1",
self.printer.config.get('Heaters', "ext1_temp_chart"))
therm_hbp = Thermistor(
path + "6_raw", "MOSFET HBP",
self.printer.config.get('Heaters', "hbp_temp_chart"))
therm_ext2 = Thermistor(
path + "5_raw", "MOSFET Ext 2",
self.printer.config.get('Heaters', "ext2_temp_chart"))
path = self.printer.config.get('Cold-ends', 'path', 0)
if os.path.exists(path):
self.printer.cold_ends.append(ColdEnd(path, "Cold End 0"))
logging.info("Found Cold end on " + path)
else:
logging.info("No cold end present in path: " + path)
# Init the 3 heaters. Argument is channel number
if self.revision == "A3":
mosfet_ext1 = Mosfet(3)
mosfet_ext2 = Mosfet(5)
mosfet_hbp = Mosfet(4)
else:
mosfet_ext1 = Mosfet(5)
mosfet_ext2 = Mosfet(3)
mosfet_hbp = Mosfet(4)
# Make extruder 1
self.printer.heaters['E'] = Extruder(
self.printer.steppers["E"], therm_ext1, mosfet_ext1, "Ext1",
self.printer.config.getboolean('Heaters', 'ext1_onoff_control'))
self.printer.heaters['E'].P = self.printer.config.getfloat(
'Heaters', "ext1_pid_p")
self.printer.heaters['E'].I = self.printer.config.getfloat(
'Heaters', "ext1_pid_i")
self.printer.heaters['E'].D = self.printer.config.getfloat(
'Heaters', "ext1_pid_d")
self.printer.heaters['E'].ok_range = self.printer.config.getfloat(
'Heaters', "ext1_ok_range")
# Make Heated Build platform
self.printer.heaters['HBP'] = HBP(
therm_hbp, mosfet_hbp,
self.printer.config.getboolean('Heaters', 'hbp_onoff_control'))
self.printer.heaters['HBP'].P = self.printer.config.getfloat(
'Heaters', "hbp_pid_p")
self.printer.heaters['HBP'].I = self.printer.config.getfloat(
'Heaters', "hbp_pid_i")
self.printer.heaters['HBP'].D = self.printer.config.getfloat(
'Heaters', "hbp_pid_d")
self.printer.heaters['HBP'].ok_range = self.printer.config.getfloat(
'Heaters', "hbp_ok_range")
# Make extruder 2.
self.printer.heaters['H'] = Extruder(
self.printer.steppers["H"], therm_ext2, mosfet_ext2, "Ext2",
self.printer.config.getboolean('Heaters', 'ext2_onoff_control'))
self.printer.heaters['H'].P = self.printer.config.getfloat(
'Heaters', "ext2_pid_p")
self.printer.heaters['H'].I = self.printer.config.getfloat(
'Heaters', "ext2_pid_i")
self.printer.heaters['H'].D = self.printer.config.getfloat(
'Heaters', "ext2_pid_d")
self.printer.heaters['H'].ok_range = self.printer.config.getfloat(
'Heaters', "ext2_ok_range")
# Init the three fans. Argument is PWM channel number
self.printer.fans = []
if self.revision == "A3":
self.printer.fans.append(Fan(0))
self.printer.fans.append(Fan(1))
self.printer.fans.append(Fan(2))
else:
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
Fan.set_PWM_frequency(100)
for f in self.printer.fans:
f.set_value(0)
# Connect the cold end 0 to fan 2
# This is very "Thing" specific, should be configurable somehow.
if len(self.printer.cold_ends):
self.printer.coolers.append(
Cooler(self.printer.cold_ends[0], self.printer.fans[2],
"Cooler0", False))
self.printer.coolers[0].ok_range = 4
self.printer.coolers[0].set_target_temperature(60)
self.printer.coolers[0].enable()
# Make a queue of commands
self.printer.commands = JoinableQueue(10)
# Make a queue of commands that should not be buffered
self.printer.unbuffered_commands = JoinableQueue(10)
# Init the path planner
Path.axis_config = int(
self.printer.config.get('Geometry', 'axis_config'))
Path.max_speeds[0] = float(
self.printer.config.get('Steppers', 'max_speed_x'))
Path.max_speeds[1] = float(
self.printer.config.get('Steppers', 'max_speed_y'))
Path.max_speeds[2] = float(
self.printer.config.get('Steppers', 'max_speed_z'))
Path.max_speeds[3] = float(
self.printer.config.get('Steppers', 'max_speed_e'))
Path.max_speeds[4] = float(
self.printer.config.get('Steppers', 'max_speed_h'))
Path.home_speed[0] = float(
self.printer.config.get('Steppers', 'home_speed_x'))
Path.home_speed[1] = float(
self.printer.config.get('Steppers', 'home_speed_y'))
Path.home_speed[2] = float(
self.printer.config.get('Steppers', 'home_speed_z'))
Path.home_speed[3] = float(
self.printer.config.get('Steppers', 'home_speed_e'))
Path.home_speed[4] = float(
self.printer.config.get('Steppers', 'home_speed_h'))
Path.steps_pr_meter[0] = self.printer.steppers["X"].get_steps_pr_meter(
)
Path.steps_pr_meter[1] = self.printer.steppers["Y"].get_steps_pr_meter(
)
Path.steps_pr_meter[2] = self.printer.steppers["Z"].get_steps_pr_meter(
)
Path.steps_pr_meter[3] = self.printer.steppers["E"].get_steps_pr_meter(
)
Path.steps_pr_meter[4] = self.printer.steppers["H"].get_steps_pr_meter(
)
dirname = os.path.dirname(os.path.realpath(__file__))
# Create the firmware compiler
pru_firmware = PruFirmware(
dirname + "/../firmware/firmware_runtime.p",
dirname + "/../firmware/firmware_runtime.bin",
dirname + "/../firmware/firmware_endstops.p",
dirname + "/../firmware/firmware_endstops.bin", self.revision,
self.printer.config, "/usr/bin/pasm")
self.printer.maxJerkXY = float(
self.printer.config.get('Steppers', 'maxJerk_xy'))
self.printer.maxJerkZ = float(
self.printer.config.get('Steppers', 'maxJerk_z'))
self.printer.maxJerkEH = float(
self.printer.config.get('Steppers', 'maxJerk_eh'))
self.printer.path_planner = PathPlanner(self.printer, pru_firmware)
travel = {}
offset = {}
i = 0
for axis in ['X', 'Y', 'Z', 'E', 'H']:
travel[axis] = self.printer.config.getfloat(
'Geometry', 'travel_' + axis.lower())
offset[axis] = self.printer.config.getfloat(
'Geometry', 'offset_' + axis.lower())
self.printer.acceleration[i] = self.printer.config.getfloat(
'Steppers', 'acceleration_' + axis.lower())
i += 1
self.printer.path_planner.travel_length = travel
self.printer.path_planner.center_offset = offset
self.printer.processor = GCodeProcessor(self.printer)
# Set up communication channels
self.printer.comms["USB"] = USB(self.printer)
self.printer.comms["Eth"] = Ethernet(self.printer)
self.printer.comms["octoprint"] = Pipe(
self.printer, "octoprint") # Pipe for Octoprint
self.printer.comms["toggle"] = Pipe(self.printer,
"toggle") # Pipe for Toggle
self.printer.comms["testing"] = Pipe(self.printer,
"testing") # Pipe for testing
self.printer.comms["testing_noret"] = Pipe(
self.printer, "testing_noret") # Pipe for testing
self.printer.comms["testing_noret"].send_response = False
self.running = True
# Start the two processes
p0 = Thread(target=self.loop, args=(self.printer.commands, "buffered"))
p1 = Thread(target=self.loop,
args=(self.printer.unbuffered_commands, "unbuffered"))
p0.daemon = True
p1.daemon = True
p0.start()
p1.start()
# Signal everything ready
logging.info("Redeem ready")
# Wait for exit signal
p0.join()
p1.join()
def __init__(self):
logging.info("Redeem initializing " + version)
self.printer = Printer()
# Parse the config
self.printer.config = CascadingConfigParser(
['/etc/redeem/default.cfg', '/etc/redeem/local.cfg'])
# Get the revision from the Config file
self.revision = self.printer.config.get('System', 'revision', "A4")
logging.info("Replicape revision " + self.revision)
# Init the end stops
EndStop.callback = self.end_stop_hit
EndStop.inputdev = self.printer.config.get("Endstops", "inputdev")
if self.revision == "A4":
self.printer.end_stops["X1"] = EndStop(
"GPIO3_21", 112, "X1",
self.printer.config.getboolean("Endstops", "invert_X1"))
self.printer.end_stops["X2"] = EndStop(
"GPIO0_30", 113, "X2",
self.printer.config.getboolean("Endstops", "invert_X2"))
self.printer.end_stops["Y1"] = EndStop(
"GPIO1_17", 114, "Y1",
self.printer.config.getboolean("Endstops", "invert_Y1"))
self.printer.end_stops["Y2"] = EndStop(
"GPIO1_19", 115, "Y2",
self.printer.config.getboolean("Endstops", "invert_Y2"))
self.printer.end_stops["Z1"] = EndStop(
"GPIO0_31", 116, "Z1",
self.printer.config.getboolean("Endstops", "invert_Z1"))
self.printer.end_stops["Z2"] = EndStop(
"GPIO0_4", 117, "Z2",
self.printer.config.getboolean("Endstops", "invert_Z2"))
else:
self.printer.end_stops["X1"] = EndStop(
"GPIO0_14", 112, "X1",
self.printer.config.getboolean("Endstops", "invert_X1"))
self.printer.end_stops["X2"] = EndStop(
"GPIO3_21", 113, "X2",
self.printer.config.getboolean("Endstops", "invert_X2"))
self.printer.end_stops["Y1"] = EndStop(
"GPIO2_2", 114, "Y1",
self.printer.config.getboolean("Endstops", "invert_Y1"))
self.printer.end_stops["Y2"] = EndStop(
"GPIO0_31", 115, "Y2",
self.printer.config.getboolean("Endstops", "invert_Y2"))
self.printer.end_stops["Z1"] = EndStop(
"GPIO0_30", 116, "Z1",
self.printer.config.getboolean("Endstops", "invert_Z1"))
self.printer.end_stops["Z2"] = EndStop(
"GPIO0_4", 117, "Z2",
self.printer.config.getboolean("Endstops", "invert_Z2"))
if self.revision == "A3":
Stepper.revision = "A3"
Stepper.ENABLED = 6
Stepper.SLEEP = 5
Stepper.RESET = 4
Stepper.DECAY = 0
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
self.printer.steppers["X"] = Stepper("GPIO0_27", "GPIO1_29", "GPIO2_4",
0, "X",
self.printer.end_stops["X1"], 0,
0)
self.printer.steppers["Y"] = Stepper("GPIO1_12", "GPIO0_22", "GPIO2_5",
1, "Y",
self.printer.end_stops["Y1"], 1,
1)
self.printer.steppers["Z"] = Stepper("GPIO0_23", "GPIO0_26",
"GPIO0_15", 2, "Z",
self.printer.end_stops["Z1"], 2,
2)
self.printer.steppers["E"] = Stepper("GPIO1_28", "GPIO1_15", "GPIO2_1",
3, "Ext1", None, 3, 3)
self.printer.steppers["H"] = Stepper("GPIO1_13", "GPIO1_14", "GPIO2_3",
4, "Ext2", None, 4, 4)
# Enable the steppers and set the current, steps pr mm and microstepping
for name, stepper in self.printer.steppers.iteritems():
stepper.set_current_value(
self.printer.config.getfloat('Steppers', 'current_' + name))
if self.printer.config.getboolean('Steppers', 'enabled_' + name):
stepper.set_enabled()
else:
stepper.set_disabled()
stepper.set_steps_pr_mm(
self.printer.config.getfloat('Steppers',
'steps_pr_mm_' + name))
stepper.set_microstepping(
self.printer.config.getint('Steppers',
'microstepping_' + name))
stepper.direction = self.printer.config.getint(
'Steppers', 'direction_' + name)
stepper.set_decay(
self.printer.config.getboolean("Steppers",
"slow_decay_" + name))
# Commit changes for the Steppers
Stepper.commit()
# Find the path of the thermistors
path = "/sys/bus/iio/devices/iio:device0/in_voltage"
# init the 3 thermistors
therm_ext1 = Thermistor(
path + "4_raw", "MOSFET Ext 1",
self.printer.config.get('Heaters', "ext1_temp_chart"))
therm_hbp = Thermistor(
path + "6_raw", "MOSFET HBP",
self.printer.config.get('Heaters', "hbp_temp_chart"))
therm_ext2 = Thermistor(
path + "5_raw", "MOSFET Ext 2",
self.printer.config.get('Heaters', "ext2_temp_chart"))
path = self.printer.config.get('Cold-ends', 'path', 0)
if os.path.exists(path):
self.printer.cold_ends.append(ColdEnd(path, "Cold End 1"))
logging.info("Found Cold end on " + path)
else:
logging.info("No cold end present in path: " + path)
# Init the 3 heaters. Argument is channel number
if self.revision == "A3":
mosfet_ext1 = Mosfet(3)
mosfet_ext2 = Mosfet(4)
mosfet_hbp = Mosfet(5)
else:
mosfet_ext1 = Mosfet(5)
mosfet_ext2 = Mosfet(3)
mosfet_hbp = Mosfet(4)
# Make extruder 1
self.printer.heaters['E'] = Extruder(
self.printer.steppers["E"], therm_ext1, mosfet_ext1, "Ext1",
self.printer.config.getboolean('Heaters', 'ext1_onoff_control'))
self.printer.heaters['E'].set_p_value(
self.printer.config.getfloat('Heaters', "ext1_pid_p"))
self.printer.heaters['E'].set_d_value(
self.printer.config.getfloat('Heaters', "ext1_pid_d"))
self.printer.heaters['E'].set_i_value(
self.printer.config.getfloat('Heaters', "ext1_pid_i"))
self.printer.heaters['E'].ok_range = self.printer.config.getfloat(
'Heaters', "ext1_ok_range")
# Make Heated Build platform
self.printer.heaters['HBP'] = HBP(
therm_hbp, mosfet_hbp,
self.printer.config.getboolean('Heaters', 'hbp_onoff_control'))
self.printer.heaters['HBP'].set_p_value(
self.printer.config.getfloat('Heaters', "hbp_pid_p"))
self.printer.heaters['HBP'].set_d_value(
self.printer.config.getfloat('Heaters', "hbp_pid_i"))
self.printer.heaters['HBP'].set_i_value(
self.printer.config.getfloat('Heaters', "hbp_pid_d"))
self.printer.heaters['HBP'].ok_range = self.printer.config.getfloat(
'Heaters', "hbp_ok_range")
# Make extruder 2.
self.printer.heaters['H'] = Extruder(
self.printer.steppers["H"], therm_ext2, mosfet_ext2, "Ext2",
self.printer.config.getboolean('Heaters', 'ext2_onoff_control'))
self.printer.heaters['H'].set_p_value(
self.printer.config.getfloat('Heaters', "ext2_pid_p"))
self.printer.heaters['H'].set_d_value(
self.printer.config.getfloat('Heaters', "ext2_pid_i"))
self.printer.heaters['H'].set_i_value(
self.printer.config.getfloat('Heaters', "ext2_pid_d"))
self.printer.heaters['H'].ok_range = self.printer.config.getfloat(
'Heaters', "ext2_ok_range")
# Init the three fans. Argument is PWM channel number
self.printer.fans = []
if self.revision == "A3":
self.printer.fans.append(Fan(1))
self.printer.fans.append(Fan(2))
self.printer.fans.append(Fan(0))
else:
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
Fan.set_PWM_frequency(100)
for f in self.printer.fans:
f.set_value(0)
# Make a queue of commands
self.commands = Queue.Queue(10)
# Set up USB, this receives messages and pushes them on the queue
self.usb = USB(self.commands)
# Virtual TTY
self.pipe = Pipe(self.commands)
#self.pipe.set_send_reponse(False)
self.ethernet = Ethernet(self.commands)
# Init the path planner
Path.axis_config = int(
self.printer.config.get('Geometry', 'axis_config'))
Path.max_speed_x = float(
self.printer.config.get('Steppers', 'max_speed_x'))
Path.max_speed_y = float(
self.printer.config.get('Steppers', 'max_speed_y'))
Path.max_speed_z = float(
self.printer.config.get('Steppers', 'max_speed_z'))
Path.max_speed_e = float(
self.printer.config.get('Steppers', 'max_speed_e'))
Path.max_speed_h = float(
self.printer.config.get('Steppers', 'max_speed_h'))
Path.home_speed_x = float(
self.printer.config.get('Steppers', 'home_speed_x'))
Path.home_speed_y = float(
self.printer.config.get('Steppers', 'home_speed_y'))
Path.home_speed_z = float(
self.printer.config.get('Steppers', 'home_speed_z'))
Path.home_speed_e = float(
self.printer.config.get('Steppers', 'home_speed_e'))
Path.home_speed_h = float(
self.printer.config.get('Steppers', 'home_speed_h'))
dirname = os.path.dirname(os.path.realpath(__file__))
# Create the firmware compiler
pru_firmware = PruFirmware(
dirname + "/../firmware/firmware_runtime.p",
dirname + "/../firmware/firmware_runtime.bin",
dirname + "/../firmware/firmware_endstops.p",
dirname + "/../firmware/firmware_endstops.bin", self.revision,
self.printer.config, "/usr/bin/pasm")
self.printer.path_planner = PathPlanner(self.printer.steppers,
pru_firmware)
self.printer.path_planner.set_acceleration(
float(self.printer.config.get('Steppers', 'acceleration')))
travel = {}
offset = {}
for axis in ['X', 'Y', 'Z']:
travel[axis] = self.printer.config.getfloat(
'Geometry', 'travel_' + axis.lower())
offset[axis] = self.printer.config.getfloat(
'Geometry', 'offset_' + axis.lower())
self.printer.path_planner.set_travel_length(travel)
self.printer.path_planner.set_center_offset(offset)
self.printer.processor = GCodeProcessor(self.printer)
# After the firmwares are loaded, the endstop states can be updated.
for k, endstop in self.printer.end_stops.iteritems():
logging.debug("Endstop " + endstop.name + " hit? : " +
str(endstop.read_value()))
self.running = True
# Signal everything ready
logging.info("Redeem ready")
from Parser.Parser import Parser
from Parser.ParseUtils import *
from command import *
import time
from datetime import timedelta
import stanfordnlp
from stanfordnlp.utils.resources import DEFAULT_MODEL_DIR
import _thread
import patterns
LOCALHOST = '127.0.0.1'
PORT = 65432
animation_delay_sec = 1.5
lang_model_dir = DEFAULT_MODEL_DIR
pp = PathPlanner()
initial_state = None
goal_state = None
start_state = None
recent_objspec = None
nextWildcardNum = 1
# Conditionally set global variables to default value
if True:
initial_state = "examples/initialState/ex2-start.txt"
goal_state = "examples/goals-PA2/ex2-goal.txt"
start_state = pp.initializeStartState(initial_state)
def do_put(obj_spec, location):
#!/usr/bin/env python
import time
from Simulator import Simulator
from GoalPlanner import GoalPlanner
from PathPlanner import PathPlanner
if __name__ == '__main__':
# Initialize the components
simulator = Simulator()
goalPlanner = GoalPlanner()
pathPlanner = PathPlanner()
timestep = 0.1
try:
while True:
now = time.time()
# Run all of the components
world = simulator.step()
goal = goalPlanner.step(world)
path = pathPlanner.step(goal)
# Wait for the next timestep
elapsed = time.time() - now
if (elapsed < timestep):
time.sleep(timestep - elapsed)
else:
def __init__(self):
""" Init """
logging.info("Redeem initializing " + version)
printer = Printer()
self.printer = printer
# check for config files
if not os.path.exists("/etc/redeem/default.cfg"):
logging.error("/etc/redeem/default.cfg does not exist, this file is required for operation")
sys.exit() # maybe use something more graceful?
# Parse the config files.
printer.config = CascadingConfigParser(
['/etc/redeem/default.cfg', '/etc/redeem/printer.cfg',
'/etc/redeem/local.cfg'])
# Find out which capes are connected
self.printer.config.parse_capes()
self.revision = self.printer.config.replicape_revision
if self.revision:
logging.info("Found Replicape rev. " + self.revision)
Path.set_axes(5)
else:
logging.warning("Oh no! No Replicape present!")
self.revision = "0A4A"
# We set it to 5 axis by default
Path.set_axes(5)
if self.printer.config.reach_revision:
Path.set_axes(8)
logging.info("Found Reach rev. "+self.printer.config.reach_revision)
# Get the revision and loglevel from the Config file
level = self.printer.config.getint('System', 'loglevel')
if level > 0:
logging.getLogger().setLevel(level)
if self.revision in ["00A4", "0A4A", "00A3"]:
PWM.set_frequency(100)
elif self.revision in ["00B1", "00B2"]:
PWM.set_frequency(1000)
# Init the Paths
Path.axis_config = printer.config.getint('Geometry', 'axis_config')
# Init the end stops
EndStop.callback = self.end_stop_hit
EndStop.inputdev = self.printer.config.get("Endstops", "inputdev")
for es in ["X1", "X2", "Y1", "Y2", "Z1", "Z2"]:
pin = self.printer.config.get("Endstops", "pin_"+es)
keycode = self.printer.config.getint("Endstops", "keycode_"+es)
invert = self.printer.config.getboolean("Endstops", "invert_"+es)
self.printer.end_stops[es] = EndStop(pin, keycode, es, invert)
# Backwards compatibility with A3
if self.revision == "00A3":
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00A3("GPIO0_27", "GPIO1_29", "GPIO2_4" , 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00A3("GPIO1_12", "GPIO0_22", "GPIO2_5" , 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00A3("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00A3("GPIO1_28", "GPIO1_15", "GPIO2_1" , 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00A3("GPIO1_13", "GPIO1_14", "GPIO2_3" , 4, "H", 4, 4)
elif self.revision == "00B1":
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00B1("GPIO0_27", "GPIO1_29", "GPIO2_4" , 11, 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00B1("GPIO1_12", "GPIO0_22", "GPIO2_5" , 12, 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00B1("GPIO0_23", "GPIO0_26", "GPIO0_15", 13, 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00B1("GPIO1_28", "GPIO1_15", "GPIO2_1" , 14, 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00B1("GPIO1_13", "GPIO1_14", "GPIO2_3" , 15, 4, "H", 4, 4)
elif self.revision == "00B2":
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00B2("GPIO0_27", "GPIO1_29", "GPIO2_4" , 11, 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00B2("GPIO1_12", "GPIO0_22", "GPIO2_5" , 12, 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00B2("GPIO0_23", "GPIO0_26", "GPIO0_15", 13, 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00B2("GPIO1_28", "GPIO1_15", "GPIO2_1" , 14, 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00B2("GPIO1_13", "GPIO1_14", "GPIO2_3" , 15, 4, "H", 4, 4)
else:
# Init the 5 Stepper motors (step, dir, fault, DAC channel, name)
printer.steppers["X"] = Stepper_00A4("GPIO0_27", "GPIO1_29", "GPIO2_4" , 0, 0, "X", 0, 0)
printer.steppers["Y"] = Stepper_00A4("GPIO1_12", "GPIO0_22", "GPIO2_5" , 1, 1, "Y", 1, 1)
printer.steppers["Z"] = Stepper_00A4("GPIO0_23", "GPIO0_26", "GPIO0_15", 2, 2, "Z", 2, 2)
printer.steppers["E"] = Stepper_00A4("GPIO1_28", "GPIO1_15", "GPIO2_1" , 3, 3, "E", 3, 3)
printer.steppers["H"] = Stepper_00A4("GPIO1_13", "GPIO1_14", "GPIO2_3" , 4, 4, "H", 4, 4)
if printer.config.reach_revision:
printer.steppers["A"] = Stepper_00A4("GPIO2_2" , "GPIO1_18", "GPIO0_14", 5, 5, "A", 5, 5)
printer.steppers["B"] = Stepper_00A4("GPIO1_14", "GPIO0_5" , "GPIO0_14", 6, 6, "B", 6, 6)
printer.steppers["C"] = Stepper_00A4("GPIO0_3" , "GPIO3_19", "GPIO0_14", 7, 7, "C", 7, 7)
# Enable the steppers and set the current, steps pr mm and
# microstepping
for name, stepper in self.printer.steppers.iteritems():
stepper.in_use = printer.config.getboolean('Steppers', 'in_use_' + name)
stepper.direction = printer.config.getint('Steppers', 'direction_' + name)
stepper.has_endstop = printer.config.getboolean('Endstops', 'has_' + name)
stepper.set_current_value(printer.config.getfloat('Steppers', 'current_' + name))
stepper.set_steps_pr_mm(printer.config.getfloat('Steppers', 'steps_pr_mm_' + name))
stepper.set_microstepping(printer.config.getint('Steppers', 'microstepping_' + name))
stepper.set_decay(printer.config.getboolean("Steppers", "slow_decay_" + name))
# Add soft end stops
Path.soft_min[Path.axis_to_index(name)] = printer.config.getfloat('Endstops', 'soft_end_stop_min_' + name)
Path.soft_max[Path.axis_to_index(name)] = printer.config.getfloat('Endstops', 'soft_end_stop_max_' + name)
# Commit changes for the Steppers
#Stepper.commit()
Stepper.printer = printer
# Delta printer setup
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
opts = ["Hez", "L", "r", "Ae", "Be", "Ce", "A_radial", "B_radial", "C_radial", "A_tangential", "B_tangential", "C_tangential" ]
for opt in opts:
Delta.__dict__[opt] = printer.config.getfloat('Delta', opt)
Delta.recalculate()
# Discover and add all DS18B20 cold ends.
import glob
paths = glob.glob("/sys/bus/w1/devices/28-*/w1_slave")
logging.debug("Found cold ends: "+str(paths))
for i, path in enumerate(paths):
self.printer.cold_ends.append(ColdEnd(path, "ds18b20-"+str(i)))
logging.info("Found Cold end "+str(i)+" on " + path)
# Make Mosfets, thermistors and extruders
heaters = ["E", "H", "HBP"]
if self.printer.config.reach_revision:
heaters.extend(["A", "B", "C"])
for e in heaters:
# Mosfets
channel = self.printer.config.getint("Heaters", "mosfet_"+e)
self.printer.mosfets[e] = Mosfet(channel)
# Thermistors
adc = self.printer.config.get("Heaters", "path_adc_"+e)
chart = self.printer.config.get("Heaters", "temp_chart_"+e)
resistance = self.printer.config.getfloat("Heaters", "resistance_"+e)
self.printer.thermistors[e] = Thermistor(adc, "MOSFET "+e, chart, resistance)
self.printer.thermistors[e].printer = printer
# Extruders
onoff = self.printer.config.getboolean('Heaters', 'onoff_'+e)
prefix = self.printer.config.get('Heaters', 'prefix_'+e)
if e != "HBP":
self.printer.heaters[e] = Extruder(
self.printer.steppers[e],
self.printer.thermistors[e],
self.printer.mosfets[e], e, onoff)
else:
self.printer.heaters[e] = HBP(
self.printer.thermistors[e],
self.printer.mosfets[e], onoff)
self.printer.heaters[e].prefix = prefix
self.printer.heaters[e].P = self.printer.config.getfloat('Heaters', 'pid_p_'+e)
self.printer.heaters[e].I = self.printer.config.getfloat('Heaters', 'pid_i_'+e)
self.printer.heaters[e].D = self.printer.config.getfloat('Heaters', 'pid_d_'+e)
# Init the three fans. Argument is PWM channel number
self.printer.fans = []
if self.revision == "00A3":
self.printer.fans.append(Fan(0))
self.printer.fans.append(Fan(1))
self.printer.fans.append(Fan(2))
elif self.revision == "0A4A":
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
elif self.revision in ["00B1", "00B2"]:
self.printer.fans.append(Fan(7))
self.printer.fans.append(Fan(8))
self.printer.fans.append(Fan(9))
self.printer.fans.append(Fan(10))
# Disable all fans
for f in self.printer.fans:
f.set_value(0)
# Init the servos
printer.servos = []
servo_nr = 0
while(printer.config.has_option("Servos", "servo_"+str(servo_nr)+"_enable")):
if printer.config.getboolean("Servos", "servo_"+str(servo_nr)+"_enable"):
channel = printer.config.get("Servos", "servo_"+str(servo_nr)+"_channel")
angle_init = printer.config.getint("Servos", "servo_"+str(servo_nr)+"_angle_init")
s = Servo(channel, 0.1, 0.2, angle_init)
printer.servos.append(s)
logging.info("Added servo "+str(servo_nr))
servo_nr += 1
# Connect thermitors to fans
for t, therm in self.printer.heaters.iteritems():
for f, fan in enumerate(self.printer.fans):
if self.printer.config.getboolean('Cold-ends', "connect-therm-{}-fan-{}".format(t, f)):
c = Cooler(therm, fan, "Cooler-{}-{}".format(t, f), False)
c.ok_range = 4
c.set_target_temperature(60)
c.enable()
self.printer.coolers.append(c)
logging.info("Cooler connects therm {} with fan {}".format(t, f))
# Connect fans to M106
printer.controlled_fans = []
for i, fan in enumerate(self.printer.fans):
if self.printer.config.getboolean('Cold-ends', "add-fan-{}-to-M106".format(i)):
printer.controlled_fans.append(self.printer.fans[i])
logging.info("Added fan {} to M106/M107".format(i))
# Connect the colds to fans
for ce, cold_end in enumerate(self.printer.cold_ends):
for f, fan in enumerate(self.printer.fans):
option = "connect-ds18b20-{}-fan-{}".format(ce, f)
if self.printer.config.has_option('Cold-ends', option):
if self.printer.config.getboolean('Cold-ends', option):
c = Cooler(cold_end, fan, "Cooler-ds18b20-{}-{}".format(ce, f), False)
c.ok_range = 4
c.set_target_temperature(60)
c.enable()
self.printer.coolers.append(c)
logging.info("Cooler connects temp sensor ds18b20 {} with fan {}".format(ce, f))
# Make a queue of commands
self.printer.commands = JoinableQueue(10)
# Make a queue of commands that should not be buffered
self.printer.sync_commands = JoinableQueue()
self.printer.unbuffered_commands = JoinableQueue(10)
# Bed compensation matrix
Path.matrix_bed_comp = printer.load_bed_compensation_matrix()
Path.matrix_bed_comp_inv = np.linalg.inv(Path.matrix_bed_comp)
logging.debug("Loaded bed compensation matrix: \n"+str(Path.matrix_bed_comp))
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
Path.max_speeds[i] = printer.config.getfloat('Planner', 'max_speed_'+axis.lower())
Path.home_speed[i] = printer.config.getfloat('Homing', 'home_speed_'+axis.lower())
Path.home_backoff_speed[i] = printer.config.getfloat('Homing', 'home_backoff_speed_'+axis.lower())
Path.home_backoff_offset[i] = printer.config.getfloat('Homing', 'home_backoff_offset_'+axis.lower())
Path.steps_pr_meter[i] = printer.steppers[axis].get_steps_pr_meter()
Path.backlash_compensation[i] = printer.config.getfloat('Steppers', 'backlash_'+axis.lower())
dirname = os.path.dirname(os.path.realpath(__file__))
# Create the firmware compiler
pru_firmware = PruFirmware(
dirname + "/firmware/firmware_runtime.p",
dirname + "/firmware/firmware_runtime.bin",
dirname + "/firmware/firmware_endstops.p",
dirname + "/firmware/firmware_endstops.bin",
self.revision, self.printer.config, "/usr/bin/pasm")
printer.maxJerkXY = printer.config.getfloat('Planner', 'maxJerk_xy')
printer.maxJerkZ = printer.config.getfloat('Planner', 'maxJerk_z')
printer.maxJerkEH = printer.config.getfloat('Planner', 'maxJerk_eh')
printer.move_cache_size = printer.config.getfloat('Planner', 'move_cache_size')
printer.print_move_buffer_wait = printer.config.getfloat('Planner', 'print_move_buffer_wait')
printer.min_buffered_move_time = printer.config.getfloat('Planner', 'min_buffered_move_time')
printer.max_buffered_move_time = printer.config.getfloat('Planner', 'max_buffered_move_time')
self.printer.processor = GCodeProcessor(self.printer)
self.printer.plugins = PluginsController(self.printer)
# Path planner
travel_default = False
center_default = False
home_default = False
# Setting acceleration before PathPlanner init
for axis in printer.steppers.keys():
printer.acceleration[Path.axis_to_index(axis)] = printer.config.getfloat(
'Planner', 'acceleration_' + axis.lower())
self.printer.path_planner = PathPlanner(self.printer, pru_firmware)
for axis in printer.steppers.keys():
i = Path.axis_to_index(axis)
# Sometimes soft_end_stop aren't defined to be at the exact hardware boundary.
# Adding 100mm for searching buffer.
if printer.config.has_option('Geometry', 'travel_' + axis.lower()):
printer.path_planner.travel_length[axis] = printer.config.getfloat('Geometry', 'travel_' + axis.lower())
else:
printer.path_planner.travel_length[axis] = (Path.soft_max[i] - Path.soft_min[i]) + .1
if axis in ['X','Y','Z']:
travel_default = True
if printer.config.has_option('Geometry', 'offset_' + axis.lower()):
printer.path_planner.center_offset[axis] = printer.config.getfloat('Geometry', 'offset_' + axis.lower())
else:
printer.path_planner.center_offset[axis] =(Path.soft_min[i] if Path.home_speed[i] > 0 else Path.soft_max[i])
if axis in ['X','Y','Z']:
center_default = True
if printer.config.has_option('Homing', 'home_' + axis.lower()):
printer.path_planner.home_pos[axis] = printer.config.getfloat('Homing', 'home_' + axis.lower())
else:
printer.path_planner.home_pos[axis] = printer.path_planner.center_offset[axis]
if axis in ['X','Y','Z']:
home_default = True
if Path.axis_config == Path.AXIS_CONFIG_DELTA:
if travel_default:
logging.warning("Axis travel (travel_*) set by soft limits, manual setup is recommended for a delta")
if center_default:
logging.warning("Axis offsets (offset_*) set by soft limits, manual setup is recommended for a delta")
if home_default:
logging.warning("Home position (home_*) set by soft limits or offset_*")
logging.info("Home position will be recalculated...")
# convert home_pos to effector space
Az = printer.path_planner.home_pos['X']
Bz = printer.path_planner.home_pos['Y']
Cz = printer.path_planner.home_pos['Z']
z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset
xyz = Delta.forward_kinematics2(Az, Bz, Cz) # effector position
# The default home_pos, provided above, is based on effector space
# coordinates for carriage positions. We need to transform these to
# get where the effector actually is.
xyz[2] += z_offset
for i, a in enumerate(['X','Y','Z']):
printer.path_planner.home_pos[a] = xyz[i]
logging.info("Home position = %s"%str(printer.path_planner.home_pos))
if Path.axis_config == Path.AXIS_CONFIG_SCARA:
if travel_default:
logging.warning("Axis travel (travel_*) set by soft limits, manual setup is recommended for a scara")
if center_default:
logging.warning("Axis offsets (offset_*) set by soft limits, manual setup is recommended for a scara")
if home_default:
logging.warning("Home position (home_*) set by soft limits or offset_*")
logging.info("Home position will be recalculated...")
# convert home_pos to effector space
#A = printer.path_planner.home_pos['X']
#B = printer.path_planner.home_pos['Y']
#C = printer.path_planner.home_pos['Z']
#z_offset = Delta.vertical_offset(Az,Bz,Cz) # vertical offset
#xyz = Scara.forward_kinematics(A, B, C) # effector position
# don't cpnvert home_pos to effector spac
# home offset is defined in cartesian space
xyz = np.array([printer.path_planner.home_pos['X'],printer.path_planner.home_pos['Y'],printer.path_planner.home_pos['Z']])
# The default home_pos, provided above, is based on effector space
# coordinates for carriage positions. We need to transform these to
# get where the effector actually is.
#xyz[2] += z_offset
for i, a in enumerate(['X','Y','Z']):
printer.path_planner.home_pos[a] = xyz[i]
logging.info("Home position = %s"%str(printer.path_planner.home_pos))
# Enable PWM and steppers
printer.enable = Enable("P9_41")
printer.enable.set_enabled()
# Set up communication channels
printer.comms["USB"] = USB(self.printer)
printer.comms["Eth"] = Ethernet(self.printer)
if Pipe.check_tty0tty() or Pipe.check_socat():
printer.comms["octoprint"] = Pipe(printer, "octoprint")
printer.comms["toggle"] = Pipe(printer, "toggle")
printer.comms["testing"] = Pipe(printer, "testing")
printer.comms["testing_noret"] = Pipe(printer, "testing_noret")
# Does not send "ok"
printer.comms["testing_noret"].send_response = False
else:
logging.warning("Neither tty0tty or socat is installed! No virtual tty pipes enabled")
