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': {} } }
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)