66 lines
1.9 KiB
Python
66 lines
1.9 KiB
Python
|
import argparse
|
||
|
|
import numpy
|
||
|
|
import time
|
||
|
|
import sys
|
||
|
|
|
||
|
|
import pypot.dynamixel
|
||
|
|
|
||
|
|
N = 1000
|
||
|
|
|
||
|
|
|
||
|
|
def read_register(dxl, register, ids):
|
||
|
|
print('\tReading {} times the {}...'.format(N, register))
|
||
|
|
|
||
|
|
t = []
|
||
|
|
getter = getattr(dxl, 'get_{}'.format(register))
|
||
|
|
|
||
|
|
for _ in range(N):
|
||
|
|
start = time.time()
|
||
|
|
getter(ids)
|
||
|
|
end = time.time()
|
||
|
|
t.append(1000 * (end - start))
|
||
|
|
|
||
|
|
print('\tTook {}ms (STD={}) per read'.format(numpy.mean(t), numpy.std(t)))
|
||
|
|
|
||
|
|
|
||
|
|
def full_test(dxl, ids):
|
||
|
|
print('Testing the communication speed with motor{} {}'.format('s' if len(ids) else '',
|
||
|
|
ids))
|
||
|
|
read_register(dxl, 'present_position', ids)
|
||
|
|
read_register(dxl, 'control_table', ids)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
available_ports = pypot.dynamixel.get_available_ports()
|
||
|
|
|
||
|
|
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||
|
|
description='Test low-level communication with Dynamixel motors.')
|
||
|
|
|
||
|
|
parser.add_argument('-p', '--ports',
|
||
|
|
type=str, nargs='+',
|
||
|
|
default=available_ports,
|
||
|
|
help='Select which port(s) to test')
|
||
|
|
|
||
|
|
parser.add_argument('-b', '--baudrate',
|
||
|
|
type=int, choices=[57600, 1000000], default=1000000,
|
||
|
|
help='Sets the baudrate')
|
||
|
|
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
for port in args.ports:
|
||
|
|
print('Now testing port {} with {}bds...'.format(port, args.baudrate))
|
||
|
|
|
||
|
|
print('Opening connection...')
|
||
|
|
with pypot.dynamixel.DxlIO(port, baudrate=args.baudrate) as dxl:
|
||
|
|
print('Ok!')
|
||
|
|
print('Scanning the bus...',)
|
||
|
|
sys.stdout.flush()
|
||
|
|
ids = dxl.scan()
|
||
|
|
print('Done!')
|
||
|
|
print('Ids found: {}'.format(ids))
|
||
|
|
|
||
|
|
full_test(dxl, ids[:1])
|
||
|
|
full_test(dxl, ids)
|
||
|
|
|
||
|
|
print('Closing port')
|