def start():
ports = pd.get_available_ports()
if not ports:
exit('No port')
print("Found ports", ports)
print('Connecting on the first available port:', ports[0])
dxl_io = pd.DxlIO(ports[0])
dxl_io.set_wheel_mode([1])
dxl_io.set_wheel_mode([2])
return dxl_io
class Motor():
#la vitesse en rpm des moteurs est un multiple de ce coeff
COEFF = 1.339
#rayon de la roue du robot en metres
R = 0.026
dxl_io = pdn.DxlIO(pdn.get_available_ports()[0])
def __init__(self, id):
self.id = id
#vitesse angulaire du robot en rad/s
self.w = 0
def rpmToRps(self, rpm):
#rpm = round per minute to rps = rad per second
return rpm * Motor.COEFF * 2 * math.pi / 60
def rpsToRpm(self, rps):
#rps = rad per second to rpm = round per minute
return rps * 60 / 2 * math.pi * Motor.COEFF
def calc_speed_motor(self):
dt = 0.05
pos1 = Motor.dxl_io.get_present_position([self.id])
time.sleep(dt)
pos2 = Motor.dxl_io.get_present_position([self.id])
delta_ang = (pos2[0] - pos1[0]) * math.pi / 180
if ((pos1[0] < 0 and pos2[0] >= 0 and pos1[0] < -90 and pos2[0] >= 90)
or (pos1[0] >= 0 and pos2[0] < 0 and pos1[0] >= 90
and pos2[0] < -90)):
#print("GROOOOOOOS COM TON CUL")
deltaPos1 = 0
deltaPos2 = 0
if (pos1[0] < 0):
deltaPos1 = pos1[0] + 180
deltaPos2 = 180 - pos2[0]
else: #if(pos1[0] >= 0)
deltaPos1 = 180 - pos1[0]
deltaPos2 = pos2[0] + 180
# print("deltaPos1 ",deltaPos1)
# print("deltaPos2 ",deltaPos2)
if (delta_ang > 0):
delta_ang = (deltaPos1 + deltaPos2) * math.pi / 180
else:
delta_ang = (-deltaPos1 - deltaPos2) * math.pi / 180
# print(self.id," en deg ",pos1[0])
# print(self.id," en deg ",pos2[0])
# print(self.id," delta angle ",delta_ang)
self.w = delta_ang / dt
def set_speed(speed):
dicts = {}
for i in range(0, len(fids)):
dicts[fids[i]] = speed
dxl.set_moving_speed(dicts)
def array_int(out):
for a in range(len(out)):
out[a] = int(out[a])
return out
port = dynamixel.get_available_ports()[0]
dxl = dynamixel.DxlIO(port)
fids = dxl.scan(ids=list(range(19)))
dxl.enable_torque(fids)
if __name__ == "__main__":
while True:
print(fids)
system('clear')
output_str = ''
for a in range(len(actions.keys())):
output_str += str(a + 1) + ' ==> ' + list(actions.keys())[a] + '\n'
output_str += 'Enter number of action to run ==> '
act = str(list(actions.keys())[int(input(output_str)) - 1])
play_action(act)
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}
# pypot dynamixel library
import pypot.dynamixel as pd
# threading for motor control
import threading
import time
import numpy as np
# get ports
# USB2AX will be the first result
ports = pd.get_available_ports()
# connect to port
motors = pd.DxlIO(ports[0], 1000000)
# get list of motors
print 'Scanning for motors...'
motor_list = motors.scan()
print 'Found motors: ' + str(motor_list)
def set_speed(motor, speed):
motors.set_moving_speed({motor: speed})
# move wheel to limits
def move_to_limit(motor, speed):
# while (abs(motors.get_moving_speed({motor})[0])<1):
# motors.set_torque_limit({motor:100})
# time.sleep(0.2)
# motors.set_moving_speed({motor: speed})
# time.sleep(0.2)
# print motors.get_moving_speed({motor})[0]
import numpy
from time import sleep
import pypot.dynamixel as dyn
from excelreader import motion
port = dyn.get_available_ports()[0]
dxl = dyn.DxlIO(port)
fids = dxl.scan(ids=list(range(19)))
def set_pos(poses):
dicts = {}
for i in range(0, len(fids)):
dicts[fids[i]] = (poses[i] - 512) * 0.29
dxl.set_goal_position(dicts)
def get_pos():
return dxl.get_present_position(tuple(fids)) #Tuple
dxl.enable_torque(fids)
print(motion("Book1.xlsx"))
dxl.disable_torque(fids)
import pypot.dynamixel as dyna
ports = dyna.get_available_ports()
if not ports:
raise IOError('no port located!')
print('ports found',ports)
print('connecting on first available port: ',ports[0])
dxl_io = dyna.DxlIO(ports[0])
motors = dxl_io.scan()
if not motors:
raise IOError('no motors connected!')
print('Connected motors: ',motors)
