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")