-
Notifications
You must be signed in to change notification settings - Fork 137
/
test_connection.py
65 lines (46 loc) · 1.92 KB
/
test_connection.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
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')