Esempio n. 1
0
async def main():
    parser = argparse.ArgumentParser(description=__doc__)

    parser.add_argument(
        '-d', '--device', type=str, default='/dev/ttyUSB0',
        help='serial device')
    parser.add_argument(
        '-b', '--baud', type=int, default=3000000, help='baud rate')

    args = parser.parse_args()

    serial = aioserial.AioSerial(port=args.device, baudrate=args.baud)
    set_serial_low_latency(serial.fd)
    manager = mp.MultiplexManager(serial)

    clients = {
        key: mp.MultiplexClient(
            manager, timeout=0.02, destination_id=key, channel=1)
        for key in [1, 2, 3] }

    for key, client in clients.items():
        client.write(b'tel stop\n')

    await manager.drain()

    await asyncio.sleep(2.0)
Esempio n. 2
0
async def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('data',
                        type=str,
                        help='file containing configuration commands')
    parser.add_argument('-d',
                        '--device',
                        type=str,
                        default='/dev/ttyUSB0',
                        help='serial device')
    parser.add_argument('-b',
                        '--baud',
                        type=int,
                        default=3000000,
                        help='baud rate')
    parser.add_argument('-t',
                        '--target',
                        type=int,
                        default=1,
                        help='destination multiplex address')
    parser.add_argument('-c',
                        '--channel',
                        type=int,
                        default=1,
                        help='destination multiples channel')
    args = parser.parse_args()

    serial = aioserial.AioSerial(port=args.device, baudrate=args.baud)
    manager = mp.MultiplexManager(serial)
    mc = mp.MultiplexClient(
        manager,
        timeout=0.3,  # "conf write" can take a long time
        destination_id=args.target,
        channel=args.channel)

    with open(args.data, 'rb') as data:
        for line in data.readlines():
            if line.strip() == '':
                continue

            print(line.strip().decode('latin1'))
            mc.write(line)
            await mc.drain()

            result = (await readline(mc)).strip()
            print('>', result.decode('latin1'))
            if result != b'OK':
                raise RuntimeError('Unknown response: ' + result)
Esempio n. 3
0
async def main():
    parser = argparse.ArgumentParser(description=__doc__)

    parser.add_argument(
        '-t', '--target', type=str, action='append', default=None,
        help='destination address(es) (default: autodiscover)')
    parser.add_argument(
        '-d', '--device', type=str, default='/dev/ttyUSB0',
        help='serial device')
    parser.add_argument(
        '-b', '--baud', type=int, default=3000000, help='baud rate')
    parser.add_argument(
        '--skip-stop', action='store_true',
        help='omit initial "tel stop"')
    parser.add_argument('-v', '--verbose', action='store_true')

    parser.add_argument('--show-plots', action='store_true',
                        help='only valid with --calibrate')
    parser.add_argument('--calibration-power', default=0.35,
                        help='voltage to use during calibration')

    # The different commands that we can do.  No more than one can be
    # specified at a time.
    group = parser.add_mutually_exclusive_group()
    group.add_argument(
        '--stop', action='store_true',
        help='command the servos to stop')
    group.add_argument(
        '--dump-config', action='store_true',
        help='emit all configuration to the console')
    group.add_argument(
        '--set-id', type=int, default=None,
        help='configure the device to use the given multiplex ID')
    group.add_argument(
        '--calibrate', action='store_true',
        help='calibrate the motor, requires full freedom of motion')
    group.add_argument(
        '--read-int8', action='append', type=str,
        help='read the given registers as int8s')
    group.add_argument(
        '--read-int16', action='append', type=str,
        help='read registers as int16s')
    group.add_argument(
        '--read-int32', action='append', type=str,
        help='read registers as int32s')
    group.add_argument(
        '--read-float', action='append', type=str,
        help='read registers as floats')
    group.add_argument(
        '--write-int8', action='append', type=str,
        help='write registers as int8')
    group.add_argument(
        '--write-int16', action='append', type=str,
        help='write registers as int16')
    group.add_argument(
        '--write-int32', action='append', type=str,
        help='write_registers as int32')
    group.add_argument(
        '--write-float', action='append', type=str,
        help='write_registers as float')
    group.add_argument(
        '--flash', type=str,
        help='write the given elf file to flash')

    args = parser.parse_args()

    args.target = expand_targets(args.target)

    if args.verbose:
        global G_VERBOSE
        G_VERBOSE = True

    serial = aioserial.AioSerial(port=args.device, baudrate=args.baud)
    set_serial_low_latency(serial.fd)

    try:
        _ = await asyncio.wait_for(serial.read(8192), 0.1)
    except asyncio.TimeoutError:
        pass

    manager = mp.MultiplexManager(serial)

    # If we don't know which target to use, then we search to see who
    # is on the bus.
    if args.target is None:
        args.target = await find_online_targets(manager)
        if len(args.target) == 0:
            print('No devices found.')
            sys.exit(1)

        print('Auto-detected device ids: ', args.target)

    clients = { key: mp.MultiplexClient(
        manager, timeout=0.02, destination_id=key, channel=1)
                for key in args.target }

    for key, client in clients.items():
        if len(clients) > 0:
            print('*** ID: {}'.format(key))

        # Read anything that might have been sitting on the channel
        # first.
        if not args.skip_stop:
            try:
                await write_command(client, b'tel stop')
                _ = await(asyncio.wait_for(readbytes(client), 0.2))
            except asyncio.TimeoutError:
                pass

        if args.stop:
            await command(client, b'd stop')

        if args.dump_config:
            await write_command(client, b'conf enumerate')

            while True:
                line = (await readline(client)).strip()
                if line.startswith(b'OK'):
                    break
                print(line.decode('utf8'))

        if args.calibrate:
            await do_calibrate(client, args)

        if args.read_int8:
            print(await _register_query(client, args.read_int8, 0))

        if args.read_int16:
            print(await _register_query(client, args.read_int16, 1))

        if args.read_int32:
            print(await _register_query(client, args.read_int32, 2))

        if args.read_float:
            print(await _register_query(client, args.read_float, 3))

        if args.write_int8:
            await _write_registers(client, args.write_int8, 0)

        if args.write_int16:
            await _write_registers(client, args.write_int16, 1)

        if args.write_int32:
            await _write_registers(client, args.write_int32, 2)

        if args.write_float:
            await _write_registers(client, args.write_float, 3)

        if args.flash:
            await _write_flash(client, args.flash)
Esempio n. 4
0
async def main():
    parser = argparse.ArgumentParser(description=__doc__)

    parser.add_argument('-d',
                        '--device',
                        type=str,
                        default='/dev/ttyUSB0',
                        help='serial device')
    parser.add_argument('-b',
                        '--baud',
                        type=int,
                        default=3000000,
                        help='baud rate')
    parser.add_argument('-o', '--output', default='/tmp')
    parser.add_argument('--really-run', action='store_true')

    args = parser.parse_args()

    output_filename = '{}/jump-{}.csv'.format(
        args.output,
        datetime.datetime.now().isoformat())
    fields = [
        'time',
        'state',
        '1_mode',
        '1_position',
        '1_velocity',
        '1_current',
        '1_temp_C',
        '2_mode',
        '2_position',
        '2_velocity',
        '2_current',
        '2_temp_C',
        '3_mode',
        '3_position',
        '3_velocity',
        '3_current',
        '3_temp_C',
    ]

    output = csv.DictWriter(open(output_filename, 'w'), fields)
    output.writeheader()

    serial = aioserial.AioSerial(port=args.device, baudrate=args.baud)
    # Empty out anything still on the bus.

    try:
        _ = await asyncio.wait_for(serial.read(8192), 0.1)
    except asyncio.TimeoutError:
        pass

    servo_set = ServoSet(serial, [1, 2, 3])

    state = State.INIT

    while True:
        data = {}
        for key in [1, 2, 3]:
            while True:
                try:
                    data[key] = await asyncio.wait_for(
                        servo_set.get_servo_stats(key), 0.04)
                    break
                except asyncio.TimeoutError:
                    # We retry infinitely.
                    pass

        now = time.time()
        print('{:.6f} {}'.format(now, state))
        for id in [1, 2, 3]:
            print(
                ' {}: {:3d} p {:7.4f}  v {:7.3f}  i {:5.1f}  t {:2.0f}'.format(
                    id, data[id].mode, data[id].unwrapped_position,
                    data[id].velocity, data[id].q_A, data[id].fet_temp_C))
        print()

        csv_data = {
            'time': now,
            'state': str(state),
            '1_mode': data[1].mode,
            '1_position': data[1].position,
            '1_velocity': data[1].velocity,
            '1_current': data[1].q_A,
            '1_temp_C': data[1].fet_temp_C,
            '2_mode': data[2].mode,
            '2_position': data[2].position,
            '2_velocity': data[2].velocity,
            '2_current': data[2].q_A,
            '2_temp_C': data[2].fet_temp_C,
            '3_mode': data[3].mode,
            '3_position': data[3].position,
            '3_velocity': data[3].velocity,
            '3_current': data[3].q_A,
            '3_temp_C': data[3].fet_temp_C,
        }
        output.writerow(csv_data)

        for key, value in data.items():
            if value.mode == 1:
                # We have a fault on one servo.  Stop the state machine.
                state = State.FAULT
                break

        if state == State.INIT:
            # Command a low current maneuver to the starting position.
            if args.really_run:
                await servo_set.command({
                    1:
                    'd pos {} 0 15\n'.format(FEMUR_START).encode('utf8'),
                    2:
                    'd pos {} 0 15\n'.format(TIBIA_START).encode('utf8'),
                    3:
                    b'd pos 0 0 40\n',
                })

            state = State.WAIT_FOR_INIT

        elif state == State.WAIT_FOR_INIT:
            # Wait for both channels to be operating and for the
            # position to be roughly there, then switch to landing.
            done = (data[1].mode == 9 and data[2].mode == 9
                    and abs(data[1].velocity) < 0.1
                    and abs(data[2].velocity) < 0.1
                    and abs(data[3].velocity) < 0.1)
            if done:
                state = State.LANDING

        elif state == State.LANDING:
            # Wait until the velocity is either zero, or the leg is
            # rising up.

            stopped = (data[1].velocity < 0.1 and data[2].velocity > -0.1)

            if stopped:
                # Start the jump.
                if args.really_run:
                    await servo_set.command({
                        1:
                        'd pos nan -9 {}\n'.format(JUMP_CURRENT).encode(
                            'latin1'),
                        2:
                        'd pos nan 9 {}\n'.format(JUMP_CURRENT).encode(
                            'latin1'),
                    })
                state = State.JUMPING
        elif state == State.FALLING:
            # Wait for contact, by watching for current to be non-zero.
            in_contact = (abs(data[1].q_A) > 10 or abs(data[2].q_A) > 10)
            if in_contact:
                # Nothing to do now, except wait for us to bottom out.
                state = State.LANDING

        elif state == State.RETRACTING:
            # Wait for servos to be roughly in position and the
            # current to be mostly stabilized.
            in_position = (
                (abs(data[1].unwrapped_position - FEMUR_START) < 0.01
                 and abs(data[2].unwrapped_position - TIBIA_START) < 0.01
                 and abs(data[1].q_A) < 5 and abs(data[2].q_A) < 5) or
                (abs(data[1].velocity) < 0.1 and abs(data[2].velocity) < 0.1))

            if in_position:
                # Switch to possibly different current commands for our landing.
                if args.really_run:
                    await servo_set.command({
                        1:
                        'd pos {} 0 {}\n'.format(
                            FEMUR_START, JUMP_CURRENT).encode('latin1'),
                        2:
                        'd pos {} 0 {}\n'.format(
                            TIBIA_START, JUMP_CURRENT).encode('latin1'),
                    })
                state = State.FALLING

        elif state == State.JUMPING:
            # Wait for servos to be vertical.
            vertical = (abs(data[1].unwrapped_position) < 0.01
                        and abs(data[2].unwrapped_position) < 0.01)
            if vertical:
                # Start our retract.
                if args.really_run:
                    await servo_set.command({
                        1:
                        'd pos {} 0 {}\n'.format(
                            FEMUR_START, RETRACT_CURRENT).encode('latin1'),
                        2:
                        'd pos {} 0 {}\n'.format(
                            TIBIA_START, RETRACT_CURRENT).encode('latin1'),
                    })
                state = State.RETRACTING
Esempio n. 5
0
async def main():
    parser = argparse.ArgumentParser(description=__doc__)

    parser.add_argument('-d',
                        '--device',
                        type=str,
                        default='/dev/ttyUSB0',
                        help='serial device')
    parser.add_argument('-b',
                        '--baud',
                        type=int,
                        default=3000000,
                        help='baud rate')
    parser.add_argument('-c', '--fdcanusb', action='store_true')
    parser.add_argument('-o', '--output', default='/tmp')
    parser.add_argument('--really-run', action='store_true')

    args = parser.parse_args()

    output_filename = '{}/jump-{}.csv'.format(
        args.output,
        datetime.datetime.now().isoformat())
    fields = [
        'time',
        'state',
        '1_mode',
        '1_position',
        '1_velocity',
        '1_current',
        '1_temp_C',
        '2_mode',
        '2_position',
        '2_velocity',
        '2_current',
        '2_temp_C',
        '3_mode',
        '3_position',
        '3_velocity',
        '3_current',
        '3_temp_C',
    ]

    output = csv.DictWriter(open(output_filename, 'w'), fields)
    output.writeheader()

    serial = aioserial.AioSerial(port=args.device, baudrate=args.baud)
    # Empty out anything still on the bus.

    try:
        _ = await asyncio.wait_for(serial.read(8192), 0.1)
    except asyncio.TimeoutError:
        pass

    IDS = [1]
    servo_set = ServoSet(serial, IDS, fdcanusb=args.fdcanusb)

    state_start_s = time.time()
    state = State.RETRACTING
    old_state = state

    while True:
        if state != old_state:
            state_start_s = time.time()
            old_state = state
        since_state_s = time.time() - state_start_s

        data = {}
        for key in IDS:
            while True:
                try:
                    data[key] = await asyncio.wait_for(
                        servo_set.get_servo_stats(key), 0.15)
                    break
                except asyncio.TimeoutError:
                    # We retry infinitely.
                    pass

        now = time.time()
        print('{:.6f} {}'.format(now, state))
        for id in [1]:
            print(
                ' {}: {:3d} p {:7.4f}  v {:7.3f}  t {:5.1f}  t {:2.0f}'.format(
                    id, data[id].mode, data[id].unwrapped_position,
                    data[id].velocity, data[id].torque_Nm,
                    data[id].fet_temp_C))
        print()

        csv_data = {
            'time': now,
            'state': str(state),
            '1_mode': data[1].mode,
            '1_position': data[1].position,
            '1_velocity': data[1].velocity,
            '1_current': data[1].q_A,
            '1_temp_C': data[1].fet_temp_C,
        }
        output.writerow(csv_data)

        for key, value in data.items():
            if value.mode == 1:
                # We have a fault on one servo.  Stop the state machine.
                state = State.FAULT
                break

        if state == State.RETRACTING:
            if args.really_run:
                await servo_set.command({
                    1:
                    'd pos nan 0.3 {} s{}\n'.format(
                        RETRACT_TORQUE, RETRACT_POS).encode('utf8'),
                })

            # Wait until enough time has passed.
            if since_state_s > 0.6:
                state = State.THROWING

        elif state == State.THROWING:
            if args.really_run:
                await servo_set.command({
                    1:
                    'd pos nan 5.0 {} s{}\n'.format(THROW_TORQUE,
                                                    THROW_POS).encode('utf8'),
                })
            if since_state_s > CATCH_DELAY_S:
                state = State.RETRACTING

        elif state == State.FAULT:
            # Nothing to do here, we stay here indefinitely.
            pass