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
def auto_open_ios(ids_groups):
available_ports = get_available_ports()
ios = {}
for ids in ids_groups:
port = find_port_for_ids(available_ports, ids)
print('Found port {} for {}.'.format(port, ids))
io = Dxl320IO(port=port, use_sync_read=True)
ios[tuple(ids)] = io
available_ports.remove(port)
return ios
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 __init__(self,port="auto"):
T.Thread.__init__(self)
self.daemon = True
if port == "auto":
# get the list of ports
listcom = D.get_available_ports()
print listcom
razorcom = []
for com in listcom:
try:
#test the port
a = S.Serial(com, 57600,timeout = 0.1)
time.sleep(3)
# do not forget to strip the zeros values
t = a.readline().strip('\x00')
a.close()
if len(t)>0:
if t[0:5] == '#YPR=':
razorcom.append(com)
except S.SerialException:
pass
else:
razorcom = [port]
if len(razorcom) == 1:
# we got the right port
print('Razor connected to '+razorcom[0]+'.')
self.s = S.Serial(razorcom[0],57600,timeout=0.1)
else:
# no port available or no razor connected
print('no Razor connected.')
self.s = None
# initialisation of all parameters (aeronautical convention with Z downward)
# euler angles in degrees
self._eul = [0.0,0.0,0.0]
# acceleration not converted
self._acc = [0.0,0.0,0.0]
# magnetic field not converted
self._mag = [0.0,0.0,0.0]
# attitude rate not converted
self._gyr = [0.0,0.0,0.0]
# control flag of the main loop
self._goon = True
# sampling period of the sensor
self._dt = 0.01
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)
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 main():
parser = ArgumentParser(description='Set a dynamixel motor to'
' a "poppy-like" configuration. '
'Only one motor should be connected!')
parser.add_argument(dest='id', type=int, help='Dynamixel target #id.')
parser.add_argument('-p',
'--port',
default=None,
choices=get_available_ports(),
help='Serial port, default: autoselect')
parser.add_argument(
'--log-level',
default='ERROR',
help='Log level : CRITICAL, ERROR, WARNING, INFO, DEBUG')
parser.add_argument('--position',
type=float,
default=0.0,
help='Position set to the motor (in degrees)')
parser.add_argument('--angle-limit',
type=float,
nargs=2,
default=[-150, 150],
help='Angle limits of the motor (in degrees)')
args = parser.parse_args()
if not (1 <= args.id <= 253):
leave('Motor id should be in range [1:253]!')
log_level = args.log_level.upper()
logger.setLevel(log_level)
# First, we make sure that there is at least one available ports
try:
port = get_available_ports()[0]
except IndexError:
leave('You need to connect at least one dynamixel port!')
print('Resetting to factory settings...')
for br in [FACTORY_BAUDRATE, TARGET_BAUDRATE]:
with DxlIO(port, br) as dxl:
dxl.factory_reset()
time.sleep(.5)
print('Done!')
print('Setting the motor to a "poppy" configuration')
with DxlIO(port, FACTORY_BAUDRATE) as dxl:
# We make sure that there was only one motor on the bus
try:
assert dxl.scan([1]) == [1]
except AssertionError:
leave('No motor found, check the connection!')
except DxlError:
leave('You should only connect one motor at'
' a time when doing factory reset!')
if args.id != 1:
print('Changing the id to {}...'.format(args.id))
dxl.change_id({1: args.id})
print('Changing the return delay time to {}...'.format(0))
dxl.set_return_delay_time({args.id: 0})
print('Changing the angle limit to {}...').format(args.angle_limit)
dxl.set_angle_limit({args.id: args.angle_limit})
print('Changing the baudrate to {}...'.format(TARGET_BAUDRATE))
dxl.change_baudrate({args.id: TARGET_BAUDRATE})
time.sleep(.5)
print('Done!')
print('Now, checking that everything went well...')
with DxlIO(port) as dxl:
try:
assert dxl.ping(args.id)
assert dxl.get_return_delay_time([args.id]) == (0, )
except AssertionError:
leave('Something went wrong with the settings of "Poppy-like"'
' motors configuration.\nThis is probably due to'
' a communication error. Please try again.')
lim = dxl.get_angle_limit([args.id])[0]
if not all(map(almost_equal, lim, args.angle_limit)):
print('Angle limit incorrect set {} instead of {}'.format(
lim, args.angle_limit))
dxl.set_goal_position({args.id: args.position})
while any(dxl.is_moving((args.id, ))):
time.sleep(.1)
pos = dxl.get_present_position((args.id, ))[0]
if not almost_equal(args.position, pos):
print('Target position not reached: {} instead of {}.'.format(
pos, args.position))
print('Done!')
return numpy.mean(dt)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-l', '--log-folder',
type=str, required=True)
parser.add_argument('-N', type=int, required=True)
args = parser.parse_args()
bp = args.log_folder
if os.path.exists(bp):
raise IOError('Folder already exists: {}'.format(bp))
os.mkdir(bp)
available_ports = get_available_ports()
port = available_ports[0]
print('Connecting on port {}.'.format(port))
dxl = DxlIO(port)
print('Scanning motors on the bus...')
ids = dxl.scan()
print('Found {}'.format(ids))
ids = ids[:1]
print('Will use id: {}'.format(ids))
if dxl.get_return_delay_time(ids)[0] != 0:
raise IOError('Make sure your motor has return delay time set to 0')
def __init__(self):
self.ports = pd.get_available_ports()
# catch no available ports
if (len(self.ports) == 0):
self.ports = ['']
self.configs = {
'woody': {
'controllers': {
'my_dxl_controller': {
'sync_read': False,
'attached_motors': ['tower', 'bases', 'head'],
'port': 'auto',
'baudrate': 1000000,
'protocol': 2
}
},
'motorgroups': {
'tower': ['tower_1', 'tower_2', 'tower_3'],
'bases': ['base'],
'head': ['ears']
},
'motors': {
'tower_1': {
'orientation': 'direct',
'type': 'XL-320',
'id': 1,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_2': {
'orientation': 'direct',
'type': 'XL-320',
'id': 2,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_3': {
'orientation': 'direct',
'type': 'XL-320',
'id': 3,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'base': {
'orientation': 'direct',
'type': 'XL-320',
'id': 4,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'ears': {
'orientation': 'direct',
'type': 'XL-320',
'id': 5,
'angle_limit': [50, 130.0],
'offset': 0.0
},
}
},
'blossom': {
'controllers': {
'mx_controller': {
'sync_read': False,
'attached_motors': ['tower', 'bases'],
'port': 'auto',
'baudrate': 1000000,
'protocol': 1
}
},
'motorgroups': {
'tower': ['tower_1', 'tower_2', 'tower_3'],
'bases': ['base']
},
'motors': {
'tower_1': {
'orientation': 'direct',
'type': 'MX-28',
'id': 1,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_2': {
'orientation': 'direct',
'type': 'MX-28',
'id': 2,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'tower_3': {
'orientation': 'direct',
'type': 'MX-28',
'id': 3,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
},
'base': {
'orientation': 'direct',
'type': 'MX-28',
'id': 4,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
}
}
},
'blossom_ears': {
'controllers': {
'xl_controller': {
'sync_read': False,
'attached_motors': ['ears'],
'port': self.ports[-1],
'baudrate': 1000000,
'protocol': 2
}
},
'motorgroups': {
'head': ['ears']
},
'motors': {
'ears': {
'orientation': 'direct',
'type': 'XL-320',
'id': 6,
'angle_limit': [-150.0, 150.0],
'offset': 0.0
}
}
},
'test': {
'controllers': {},
'motorgroups': {},
'motors': {}
}
}
return numpy.mean(dt)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-l', '--log-folder', type=str, required=True)
parser.add_argument('-N', type=int, required=True)
args = parser.parse_args()
bp = args.log_folder
if os.path.exists(bp):
raise IOError('Folder already exists: {}'.format(bp))
os.mkdir(bp)
available_ports = get_available_ports()
port = available_ports[0]
print('Connecting on port {}.'.format(port))
dxl = DxlIO(port)
print('Scanning motors on the bus...')
ids = dxl.scan()
print('Found {}'.format(ids))
ids = ids[:1]
print('Will use id: {}'.format(ids))
if dxl.get_return_delay_time(ids)[0] != 0:
raise IOError('Make sure your motor has return delay time set to 0')
def main():
parser = ArgumentParser(description='Set a dynamixel motor to'
' a "poppy-like" configuration. '
'Only one motor should be connected!')
parser.add_argument(dest='id', type=int,
help='Dynamixel target #id.')
parser.add_argument('-p', '--port', default=None,
choices=get_available_ports(),
help='Serial port, default: autoselect')
parser.add_argument('--log-level', default='ERROR',
help='Log level : CRITICAL, ERROR, WARNING, INFO, DEBUG')
parser.add_argument('--position', type=float, default=0.0,
help='Position set to the motor (in degrees)')
parser.add_argument('--angle-limit', type=float, nargs=2, default=[-150, 150],
help='Angle limits of the motor (in degrees)')
args = parser.parse_args()
if not (1 <= args.id <= 253):
leave('Motor id should be in range [1:253]!')
log_level = args.log_level.upper()
logger.setLevel(log_level)
# First, we make sure that there is at least one available ports
try:
port = get_available_ports()[0]
except IndexError:
leave('You need to connect at least one dynamixel port!')
print('Resetting to factory settings...')
for br in [FACTORY_BAUDRATE, TARGET_BAUDRATE]:
with DxlIO(port, br) as dxl:
dxl.factory_reset()
time.sleep(.5)
print('Done!')
print('Setting the motor to a "poppy" configuration')
with DxlIO(port, FACTORY_BAUDRATE) as dxl:
# We make sure that there was only one motor on the bus
try:
assert dxl.scan([1]) == [1]
except AssertionError:
leave('No motor found, check the connection!')
except DxlError:
leave('You should only connect one motor at'
' a time when doing factory reset!')
if args.id != 1:
print('Changing the id to {}...'.format(args.id))
dxl.change_id({1: args.id})
print('Changing the return delay time to {}...'.format(0))
dxl.set_return_delay_time({args.id: 0})
time.sleep(.5)
print('Changing the angle limit to {}...').format(args.angle_limit)
dxl.set_angle_limit({args.id: args.angle_limit})
time.sleep(.5)
print('Changing the baudrate to {}...'.format(TARGET_BAUDRATE))
dxl.change_baudrate({args.id: TARGET_BAUDRATE})
time.sleep(.5)
print('Done!')
print('Now, checking that everything went well...')
with DxlIO(port) as dxl:
try:
assert dxl.ping(args.id)
assert dxl.get_return_delay_time([args.id]) == (0, )
except AssertionError:
leave('Something went wrong with the settings of "Poppy-like"'
' motors configuration.\nThis is probably due to'
' a communication error. Please try again.')
lim = dxl.get_angle_limit([args.id])[0]
if not all(map(almost_equal, lim, args.angle_limit)):
print('Angle limit incorrect set {} instead of {}'.format(
lim, args.angle_limit))
dxl.set_goal_position({args.id: args.position})
while any(dxl.is_moving((args.id, ))):
time.sleep(.1)
pos = dxl.get_present_position((args.id, ))[0]
if not almost_equal(args.position, pos):
print('Target position not reached: {} instead of {}.'.format(
pos, args.position))
print('Done!')
def main():
available_ports = get_available_ports()
default_port = available_ports[0] if available_ports else None
parser = ArgumentParser(description='Configuration tool for dynamixel motors '
'WARNING: ONLY ONE MOTOR SHOULD BE '
'CONNECTED TO THE BUS WHEN CONFIGURING!',
formatter_class=ArgumentDefaultsHelpFormatter)
parser.add_argument('--id', type=int, required=True,
help='Chosen motor id.')
parser.add_argument('--type', type=str, required=True,
choices=dynamixelModels.values(),
help='Type of the motor to configure.')
parser.add_argument('--port', type=str,
choices=available_ports, default=default_port,
help='Serial port connected to the motor.')
parser.add_argument('--return-delay-time', type=int,
help='Set new return delay time.')
parser.add_argument('--wheel-mode', type=bool, default=False,
help='Set wheel mode.')
parser.add_argument('--angle-limit', type=float, nargs=2,
help='Set new angle limit.')
parser.add_argument('--goto-zero', action='store_true',
help='Go to zero position after configuring the motor')
args = parser.parse_args()
check(1 <= args.id <= 253,
'Motor id must be in range [1:253]')
check(available_ports,
'Could not find an available serial port!')
protocol = 2 if args.type in 'XL-320' else 1
DxlIOPort = DxlIO if protocol == 1 else Dxl320IO
# Factory Reset
print('Factory reset...')
if protocol == 1:
for br in [57600, 1000000]:
with DxlIO(args.port, baudrate=br) as io:
io.factory_reset()
else:
with Dxl320IO(args.port, baudrate=1000000, timeout=0.01) as io:
io.factory_reset(ids=range(253))
print('Done!')
factory_baudrate = 57600 if args.type.startswith('MX') else 1000000
# Wait for the motor to "reboot..."
for _ in range(10):
with DxlIOPort(args.port, baudrate=factory_baudrate) as io:
if io.ping(1):
break
time.sleep(.5)
else:
print('Could not communicate with the motor...')
print('Make sure one (and only one) is connected and try again')
sys.exit(1)
# Switch to 1M bauds
if args.type.startswith('MX') or args.type.startswith('SR'):
print('Changing to 1M bauds...')
with DxlIO(args.port, baudrate=factory_baudrate) as io:
io.change_baudrate({1: 1000000})
time.sleep(.5)
print('Done!')
# Change id
print('Changing id to {}...'.format(args.id))
if args.id != 1:
with DxlIOPort(args.port) as io:
io.change_id({1: args.id})
time.sleep(.5)
check(io.ping(args.id),
'Could not change id to {}'.format(args.id))
print('Done!')
# Set return delay time
if args.return_delay_time is not None:
print('Changing return delay time to {}...'.format(args.return_delay_time))
with DxlIOPort(args.port) as io:
io.set_return_delay_time({args.id: args.return_delay_time})
time.sleep(.5)
check(io.get_return_delay_time([args.id])[0] == args.return_delay_time,
'Could not set return delay time to {}'.format(args.return_delay_time))
print('Done!')
# Set wheel Mode
if args.wheel_mode == True:
print('Set wheel mode')
with DxlIOPort(args.port) as io:
io.set_control_mode({args.id :'wheel'})
time.sleep(.5)
check(io.get_control_mode([args.id])[0] == 'wheel',
'Could not set wheel Mode')
print('Done!')
# Set Angle Limit
if args.angle_limit is not None:
print('Changing angle limit to {}...'.format(args.angle_limit))
with DxlIOPort(args.port) as io:
io.set_angle_limit({args.id: args.angle_limit})
time.sleep(.5)
check(all(map(lambda p1, p2: abs(p1 - p2) < 1.,
io.get_angle_limit([args.id])[0],
args.angle_limit)),
'Could not change angle limit to {}'.format(args.angle_limit))
print('Done!')
# GOTO ZERO
if args.goto_zero:
print('Going to position 0...')
with DxlIOPort(args.port) as io:
io.set_moving_speed({args.id: 100.0})
io.set_goal_position({args.id: 0.0})
time.sleep(2.0)
check(abs(io.get_present_position([args.id])[0]) < 5,
'Could not go to 0 position')
print('Done!')
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)