import pypot.dynamixel as pd
# get ports and start connection to motors
ports = pd.get_available_ports()
motors = pd.Dxl320IO(ports[0], 1000000)
motor_id = motors.scan(range(20))
# only work with one motor at a time
if (len(motor_id) > 1):
print("Only connect one motor at a time!")
quit()
motor_id = motor_id[0]
print("Motor " + str(motor_id) + " found!")
# change ID
new_id = int(raw_input("Enter motor ID (5 for ear): "))
if (new_id != motor_id):
motors.change_id({motor_id: new_id})
print("Motor ID changed from " + str(motor_id) + " to " + str(new_id))
motor_id = new_id
# set motor to 100
motors.set_goal_position({motor_id: 100})
raw_input("Motor position: 100; attach horn, press 'Enter' to continue. ")
# set motor to 100
motors.set_goal_position({motor_id: 150})
raw_input(
"Motor position: 150; tighten string around ear so that it's lined against the ear holder, press 'Enter' to continue. "
)
def get_configs(self, names):
"""
assign unique ports to a list of names for as many ports as we have available
"""
# test config for debugging without a robot
if (names[0] == 'test'):
return {names[0]: self.configs[names[0]]}
# get configs for all robots
configs = []
for name in names:
if name in self.configs:
configs.append((name, self.configs[name]))
# get IDs for connected motors
used_configs = []
for port in self.ports:
if port == "":
print("No ports available")
sys.exit(1)
try:
if (names[0] != 'blossom'):
dxl_io = pd.Dxl320IO(port)
else:
dxl_io = pd.DxlIO(port)
scanned_ids = dxl_io.scan(range(8))
# handle unopenable serial port
except SerialException as e:
print(e)
print("Error opening port, try:")
print("sudo chmod 777 " + port)
sys.exit(1)
# general exception
except Exception as e:
print(
"general exception caught (please update the except statement with the exception)",
e)
scanned_ids = []
# print found motors
if (len(scanned_ids) == 0):
print("No motors found on %s" % port)
continue
else:
print("Motors for %s:" % port, scanned_ids)
# iterate through all robot configs
for i in range(len(configs)):
name, config = configs[i]
# MH: scanning doesn't seem to work for the usb hub. Doing this for now since we don't use multiple bots, but if we ever do...valid_port_for_robot probably won't work
valid_port = (len(names) == 1 or self.valid_port_for_robot(
scanned_ids, config))
# remove missing motors
config = self.return_valid_motors(scanned_ids, config)
# iterate through all configs
if configs[i] not in used_configs and valid_port:
controller = config['controllers'].keys()[0]
config['controllers'][controller]['port'] = port
used_configs.append(configs[i])
print("Assigning %s to port %s" % (name, port))
break
else:
print("No robot found for port", port)
return {n: c for n, c in used_configs}
