/
__init__.py
141 lines (101 loc) · 4.46 KB
/
__init__.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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
import sys
import platform
import glob
import logging
import serial.tools.list_ports
from .io import DxlIO, Dxl320IO
from .error import BaseErrorHandler
from .syncloop import BaseDxlController
from .motor import DxlMXMotor, DxlAXRXMotor, DxlXL320Motor, DxlSRMotor
from .io.abstract_io import DxlError
from ..robot import Robot
logger = logging.getLogger(__name__)
def _get_available_ports():
""" Tries to find the available serial ports on your system. """
if platform.system() == 'Darwin':
return glob.glob('/dev/tty.usb*')
elif platform.system() == 'Linux':
return glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyAMA*')
elif sys.platform.lower() == 'cygwin':
return glob.glob('/dev/com*')
elif platform.system() == 'Windows':
import winreg
import itertools
ports = []
path = 'HARDWARE\\DEVICEMAP\\SERIALCOMM'
key = winreg.OpenKey(winreg.HKEY_LOCAL_MACHINE, path)
for i in itertools.count():
try:
ports.append(str(winreg.EnumValue(key, i)[1]))
except WindowsError:
return ports
else:
raise EnvironmentError('{} is an unsupported platform, cannot find serial ports!'.format(platform.system()))
return []
def get_available_ports(only_free=False):
ports = _get_available_ports()
if only_free:
ports = [port for port in ports if port not in DxlIO.get_used_ports()]
return ports
def get_port_vendor_info(port=None):
""" Return vendor informations of a usb2serial device.
It may depends on the Operating System.
:param string port: port of the usb2serial device
:Example:
Result with a USB2Dynamixel on Linux:
In [1]: import pypot.dynamixel
In [2]: pypot.dynamixel.get_port_vendor_info('/dev/ttyUSB0')
Out[2]: 'USB VID:PID=0403:6001 SNR=A7005LKE' """
port_info_dict = dict((x[0], x[2]) for x in serial.tools.list_ports.comports())
return port_info_dict[port] if port is not None else port_info_dict
def find_port(ids, strict=True):
""" Find the port with the specified attached motor ids.
:param list ids: list of motor ids to find
:param bool strict: specify if all ids should be find (when set to False, only half motor must be found)
.. warning:: If two (or more) ports are attached to the same list of motor ids the first match will be returned.
"""
ids_founds = []
for port in get_available_ports():
for DxlIOCls in (DxlIO, Dxl320IO):
try:
with DxlIOCls(port) as dxl:
_ids_founds = dxl.scan(ids)
ids_founds += _ids_founds
if strict and sorted(_ids_founds) == sorted(ids):
return port
if not strict and len(_ids_founds) >= len(ids) / 2:
logger.warning('Missing ids: {}'.format(ids, list(set(ids) - set(_ids_founds))))
return port
if len(ids_founds) > 0:
logger.warning('Port:{} ids found:{}'.format(port, _ids_founds))
except DxlError:
logger.warning('DxlError on port {}'.format(port))
continue
missing = list(set(ids) - set(ids_founds))
if len(missing) == 0:
raise ValueError('All motors have been found but they are not connected as specified in the configuration file, please check connections')
raise IndexError('No suitable port found for ids {}. These ids are missing {} !'.format(ids, missing))
def autodetect_robot():
""" Creates a :class:`~pypot.robot.robot.Robot` by detecting dynamixel motors on all available ports. """
motor_controllers = []
for port in get_available_ports():
for DxlIOCls in (DxlIO, Dxl320IO):
dxl_io = DxlIOCls(port)
ids = dxl_io.scan()
if not ids:
dxl_io.close()
continue
models = dxl_io.get_model(ids)
motorcls = {
'MX': DxlMXMotor,
'RX': DxlAXRXMotor,
'AX': DxlAXRXMotor,
'XL': DxlXL320Motor,
'SR': DxlSRMotor,
}
motors = [motorcls[model[:2]](id, model=model)
for id, model in zip(ids, models)]
c = BaseDxlController(dxl_io, motors)
motor_controllers.append(c)
break
return Robot(motor_controllers)