Exemplo n.º 1
0
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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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
Exemplo n.º 4
0
 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
Exemplo n.º 5
0

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)
Exemplo n.º 6
0
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. "
)
Exemplo n.º 7
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})

        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!')
Exemplo n.º 8
0
    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')
Exemplo n.º 9
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': {}
         }
     }
Exemplo n.º 10
0
    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')
Exemplo n.º 11
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!')
Exemplo n.º 12
0
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!')
Exemplo n.º 13
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)