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)
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)
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)
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
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
