Skip to content

Commit 3f5cd7f

Browse files
dbddbd
committed
dynamically create getters and setters
1 parent 883e193 commit 3f5cd7f

File tree

4 files changed

+171
-89
lines changed

4 files changed

+171
-89
lines changed

‎dynamixel/devices/__init__.py‎

Lines changed: 29 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -10,16 +10,37 @@
1010
imports = [("xl430w250t", "XL430_W250_T"), ("ax12a", "AX12A")]
1111

1212

13-
def make_getter(addr, size):
14-
def getter(self, _addr=addr, _size=size):
15-
return self.readControlTableItem(_addr, _size)
13+
def make_getter(ct):
14+
addr, length, _, _, defaultUnit = ct
15+
16+
def getter(self, _addr=addr, _length=length, unit=None):
17+
data = None
18+
res = self.readControlTableItem(_addr, _length)
19+
unit = unit or self.unit or defaultUnit
20+
if res.ok:
21+
data = self.convertFromNegative(res.data, _length)
22+
data = self.convertRaw(data, unit)
23+
res.data = data
24+
return res
1625

1726
return getter
1827

1928

20-
def make_setter(addr, size):
21-
def setter(self, data, _addr=addr, _size=size):
22-
return self.writeControlTableItem(_addr, _size, data)
29+
def make_setter(ct):
30+
addr, length, _, limits, defaultUnit = ct
31+
32+
def setter(self, data, _addr=addr, _length=length, _limits=limits, unit=None):
33+
unit = unit or self.unit or defaultUnit
34+
data = self.convertUnits(data, unit)
35+
if data < 0:
36+
data = self.convertToNegative(data)
37+
if isinstance(_limits, list):
38+
assert data in _limits, f"{data} should be one of {_limits}"
39+
if isinstance(limits, tuple):
40+
assert _limits[0] <= data <= _limits[1], (
41+
f"{data} should be within {_limits[0]} and {_limits[1]}"
42+
)
43+
return self.writeControlTableItem(_addr, _length, data)
2344

2445
return setter
2546

@@ -34,7 +55,7 @@ def setter(self, data, _addr=addr, _size=size):
3455
baseName = "".join([word[0] + word[1:].lower() for word in key.split("_")])
3556
if ct.writable:
3657
methodName = f"set{baseName}"
37-
setattr(cls, methodName, make_setter(*ct[:2]))
58+
setattr(cls, methodName, make_setter(ct))
3859

3960
methodName = f"get{baseName}"
40-
setattr(cls, methodName, make_getter(*ct[:2]))
61+
setattr(cls, methodName, make_getter(ct))

‎dynamixel/devices/ax12a.py‎

Lines changed: 36 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
from dynamixel import utils
99
from dynamixel.protocol import Protocol1, Response
10-
from dynamixel.servo import ControlTableItem, Servo, controlTable, paramUnit
10+
from dynamixel.servo import ControlTableItem, Servo, controlTable, units
1111

1212

1313
class operatingMode:
@@ -18,52 +18,63 @@ class operatingMode:
1818
class ControlTable(controlTable):
1919
MODEL_NUMBER = ControlTableItem(0, 2, False)
2020
FIRMWARE_VERSION = ControlTableItem(2, 1, False)
21-
ID = ControlTableItem(3, 1, True)
22-
BAUD = ControlTableItem(4, 1, True)
23-
RETURN_DELAY_TIME = ControlTableItem(5, 1, True)
24-
CW_ANGLE_LIMIT = ControlTableItem(6, 2, True)
25-
CCW_ANGLE_LIMIT = ControlTableItem(8, 2, True)
26-
TEMPERATURE_LIMIT = ControlTableItem(11, 1, True)
27-
MIN_VOLTAGE_LIMIT = ControlTableItem(12, 1, True)
28-
MAX_VOLTAGE_LIMIT = ControlTableItem(13, 1, True)
29-
MAX_TORQUE = ControlTableItem(14, 2, True)
30-
STATUS_RETURN_LEVEL = ControlTableItem(16, 1, True)
21+
ID = ControlTableItem(3, 1, True, (0, 253))
22+
BAUD = ControlTableItem(4, 1, True, [1, 3, 4, 7, 9, 16, 34, 103, 207], units.BAUD)
23+
RETURN_DELAY_TIME = ControlTableItem(5, 1, True, (0, 254))
24+
CW_ANGLE_LIMIT = ControlTableItem(6, 2, True, (0, 1023))
25+
CCW_ANGLE_LIMIT = ControlTableItem(8, 2, True, (0, 1023))
26+
TEMPERATURE_LIMIT = ControlTableItem(11, 1, True, (0, 100))
27+
MIN_VOLTAGE_LIMIT = ControlTableItem(12, 1, True, (50, 160), units.VOLTAGE)
28+
MAX_VOLTAGE_LIMIT = ControlTableItem(13, 1, True, (50, 160), units.VOLTAGE)
29+
MAX_TORQUE = ControlTableItem(14, 2, True, (0, 1023))
30+
STATUS_RETURN_LEVEL = ControlTableItem(16, 1, True, [0, 1, 2])
3131
ALARM_LED = ControlTableItem(17, 1, True)
3232
SHUTDOWN = ControlTableItem(18, 1, True)
3333

34-
TORQUE_ENABLE = ControlTableItem(24, 1, True)
35-
LED = ControlTableItem(25, 1, True)
36-
CW_COMPLIANCE_MARGIN = ControlTableItem(26, 1, True)
37-
CCW_COMPLIANCE_MARGIN = ControlTableItem(27, 1, True)
38-
CW_COMPLIANCE_SLOPE = ControlTableItem(28, 1, True)
39-
CCW_COMPLIANCE_SLOPE = ControlTableItem(29, 1, True)
40-
GOAL_POSITION = ControlTableItem(30, 2, True)
41-
MOVING_SPEED = ControlTableItem(32, 2, True)
42-
TORQUE_LIMIT = ControlTableItem(34, 2, True)
43-
PRESENT_POSITION = ControlTableItem(36, 2, False)
34+
TORQUE_ENABLE = ControlTableItem(24, 1, True, [0, 1])
35+
LED = ControlTableItem(25, 1, True, [0, 1])
36+
CW_COMPLIANCE_MARGIN = ControlTableItem(26, 1, True, (0, 255))
37+
CCW_COMPLIANCE_MARGIN = ControlTableItem(27, 1, True, (0, 255))
38+
CW_COMPLIANCE_SLOPE = ControlTableItem(28, 1, True, (0, 255))
39+
CCW_COMPLIANCE_SLOPE = ControlTableItem(29, 1, True, (0, 255))
40+
GOAL_POSITION = ControlTableItem(30, 2, True, (0, 1023), units.DEGREE)
41+
MOVING_SPEED = ControlTableItem(32, 2, True, (0, 2047), units.RPM)
42+
TORQUE_LIMIT = ControlTableItem(34, 2, True, (0, 1023))
43+
PRESENT_POSITION = ControlTableItem(36, 2, False, units.DEGREE)
4444
PRESENT_SPEED = ControlTableItem(38, 2, False)
4545
PRESENT_LOAD = ControlTableItem(40, 2, False)
4646
PRESENT_VOLTAGE = ControlTableItem(42, 1, False)
4747
PRESENT_TEMPERATURE = ControlTableItem(43, 1, False)
4848
REGISTERED = ControlTableItem(44, 1, False)
4949
MOVING = ControlTableItem(46, 1, False)
50-
LOCK = ControlTableItem(47, 1, True)
51-
PUNCH = ControlTableItem(48, 2, True)
50+
LOCK = ControlTableItem(47, 1, True, [0, 1])
51+
PUNCH = ControlTableItem(48, 2, True, [0, 1])
5252

5353

5454
class AX12A(Servo):
5555
CONTROL_TABLE = ControlTable
56-
PARAM_UNIT = paramUnit
5756
OPERATING_MODE = operatingMode
5857

59-
def __init__(self, *args, unit=PARAM_UNIT.UNIT_DEGREE, **kwargs):
58+
def __init__(self, *args, unit=None, **kwargs):
6059
super().__init__(*args, **kwargs)
6160
self.protocol = Protocol1(**kwargs)
6261
self.unit = unit
6362
self.resolution = 1024
6463
self.torqueEnabled = False
6564
self.moving = False
6665
self.presentPosition = 0
66+
self._rpm = 0.111
67+
self.bauds = {
68+
1: 1000000,
69+
3: 500000,
70+
4: 400000,
71+
7: 250000,
72+
9: 200000,
73+
16: 115200,
74+
34: 57600,
75+
103: 19200,
76+
207: 9600,
77+
}
6778

6879
async def run(self):
6980
while True:

‎dynamixel/devices/xl430w250t.py‎

Lines changed: 58 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77

88
from dynamixel import utils
99
from dynamixel.protocol import Protocol2, Response
10-
from dynamixel.servo import ControlTableItem, Servo, controlTable, paramUnit
10+
from dynamixel.servo import ControlTableItem, Servo, controlTable, units
1111

1212

1313
class operatingMode:
@@ -21,42 +21,43 @@ class ControlTable(controlTable):
2121
MODEL_NUMBER = ControlTableItem(0, 2, False)
2222
MODEL_INFORMATION = ControlTableItem(2, 4, False)
2323
FIRMWARE_VERSION = ControlTableItem(6, 1, False)
24-
ID = ControlTableItem(7, 1, True)
25-
BAUD = ControlTableItem(8, 1, True)
26-
RETURN_DELAY_TIME = ControlTableItem(9, 1, True)
27-
DRIVE_MODE = ControlTableItem(10, 1, True)
28-
OPERATING_MODE = ControlTableItem(11, 1, True)
29-
SECONDARY_SHADOW_ID = ControlTableItem(12, 1, True)
30-
PROTOCOL_TYPE = ControlTableItem(13, 1, True)
31-
HOMING_OFFSET = ControlTableItem(20, 4, True)
32-
MOVING_THRESHOLD = ControlTableItem(24, 4, True)
33-
TEMPERATURE_LIMIT = ControlTableItem(31, 1, True)
34-
MAX_VOLTAGE_LIMIT = ControlTableItem(32, 2, True)
35-
MIN_VOLTAGE_LIMIT = ControlTableItem(34, 2, True)
36-
PWM_LIMIT = ControlTableItem(36, 2, True)
37-
VELOCITY_LIMIT = ControlTableItem(44, 4, True)
38-
MAX_POSITION_LIMIT = ControlTableItem(48, 4, True)
39-
MIN_POSITION_LIMIT = ControlTableItem(52, 4, True)
24+
ID = ControlTableItem(7, 1, True, (0, 252))
25+
BAUD = ControlTableItem(8, 1, True, (0, 7), units.BAUD)
26+
RETURN_DELAY_TIME = ControlTableItem(9, 1, True, (0, 254))
27+
DRIVE_MODE = ControlTableItem(10, 1, True, (0, 13))
28+
OPERATING_MODE = ControlTableItem(11, 1, True, [1, 3, 4, 16])
29+
SECONDARY_SHADOW_ID = ControlTableItem(12, 1, True, (0, 255))
30+
PROTOCOL_TYPE = ControlTableItem(13, 1, True, [1, 2])
31+
HOMING_OFFSET = ControlTableItem(20, 4, True, (-1044479, 1044479))
32+
MOVING_THRESHOLD = ControlTableItem(24, 4, True, (0, 1023))
33+
TEMPERATURE_LIMIT = ControlTableItem(31, 1, True, (0, 100))
34+
MAX_VOLTAGE_LIMIT = ControlTableItem(32, 2, True, (65, 140), units.VOLTAGE)
35+
MIN_VOLTAGE_LIMIT = ControlTableItem(34, 2, True, (65, 140), units.VOLTAGE)
36+
PWM_LIMIT = ControlTableItem(36, 2, True, (0, 885))
37+
VELOCITY_LIMIT = ControlTableItem(44, 4, True, (0, 1024), units.RPM)
38+
MAX_POSITION_LIMIT = ControlTableItem(48, 4, True, (0, 4095))
39+
MIN_POSITION_LIMIT = ControlTableItem(52, 4, True, (0, 4095))
4040
STARTUP_CONFIGURATION = ControlTableItem(60, 1, True)
4141
SHUTDOWN = ControlTableItem(63, 1, True)
42-
TORQUE_ENABLE = ControlTableItem(64, 1, True)
43-
LED = ControlTableItem(65, 1, True)
44-
STATUS_RETURN_LEVEL = ControlTableItem(68, 1, True)
45-
REGISTERED_INSTRUCTION = ControlTableItem(69, 1, False)
42+
43+
TORQUE_ENABLE = ControlTableItem(64, 1, True, [0, 1])
44+
LED = ControlTableItem(65, 1, True, [0, 1])
45+
STATUS_RETURN_LEVEL = ControlTableItem(68, 1, True, [0, 1, 2])
46+
REGISTERED_INSTRUCTION = ControlTableItem(69, 1, False, [0, 1])
4647
HARDWARE_ERROR_STATUS = ControlTableItem(70, 1, False)
47-
VELOCITY_I_GAIN = ControlTableItem(76, 2, True)
48-
VELOCITY_P_GAIN = ControlTableItem(78, 2, True)
49-
POSITION_D_GAIN = ControlTableItem(80, 2, True)
50-
POSITION_I_GAIN = ControlTableItem(82, 2, True)
51-
POSITION_P_GAIN = ControlTableItem(84, 2, True)
52-
FEEDFORWARD_2ND_GAIN = ControlTableItem(88, 2, True)
53-
FEEDFORWARD_1ST_GAIN = ControlTableItem(90, 2, True)
54-
BUS_WATCHDOG = ControlTableItem(98, 1, True)
55-
GOAL_PWM = ControlTableItem(100, 2, True)
56-
GOAL_VELOCITY = ControlTableItem(104, 4, True)
57-
PROFILE_ACCELERATION = ControlTableItem(108, 4, True)
58-
PROFILE_VELOCITY = ControlTableItem(112, 4, True)
59-
GOAL_POSITION = ControlTableItem(116, 4, True)
48+
VELOCITY_I_GAIN = ControlTableItem(76, 2, True, (0, 16383))
49+
VELOCITY_P_GAIN = ControlTableItem(78, 2, True, (0, 16383))
50+
POSITION_D_GAIN = ControlTableItem(80, 2, True, (0, 16383))
51+
POSITION_I_GAIN = ControlTableItem(82, 2, True, (0, 16383))
52+
POSITION_P_GAIN = ControlTableItem(84, 2, True, (0, 16383))
53+
FEEDFORWARD_2ND_GAIN = ControlTableItem(88, 2, True, (0, 16383))
54+
FEEDFORWARD_1ST_GAIN = ControlTableItem(90, 2, True, (0, 16383))
55+
BUS_WATCHDOG = ControlTableItem(98, 1, True, (0, 127))
56+
GOAL_PWM = ControlTableItem(100, 2, True, (-885, 885))
57+
GOAL_VELOCITY = ControlTableItem(104, 4, True, (-1024, 1024))
58+
PROFILE_ACCELERATION = ControlTableItem(108, 4, True, (0, 32767))
59+
PROFILE_VELOCITY = ControlTableItem(112, 4, True, (0, 32767))
60+
GOAL_POSITION = ControlTableItem(116, 4, True, (-1048575, 1048575), units.DEGREE)
6061
REALTIME_TICK = ControlTableItem(120, 2, False)
6162
MOVING = ControlTableItem(122, 1, False)
6263
MOVING_STATUS = ControlTableItem(123, 1, False)
@@ -71,30 +72,29 @@ class ControlTable(controlTable):
7172
BACKUP_READY = ControlTableItem(147, 1, False)
7273

7374

74-
def convertToNegative(value, length):
75-
if value < 0:
76-
maxInt = int.from_bytes(bytes([0xFF] * length), "little")
77-
maxInt += value
78-
return list(maxInt.to_bytes(length, "little"))
79-
80-
81-
def convertFromNegative(value, length):
82-
return utils.twosComplement(value, length)
83-
84-
8575
class XL430_W250_T(Servo):
8676
CONTROL_TABLE = ControlTable
87-
PARAM_UNIT = paramUnit
8877
OPERATING_MODE = operatingMode
8978

90-
def __init__(self, *args, unit=PARAM_UNIT.UNIT_DEGREE, **kwargs):
79+
def __init__(self, *args, unit=None, **kwargs):
9180
super().__init__(*args, **kwargs)
9281
self.unit = unit
9382
self.protocol = Protocol2(**kwargs)
9483
self.resolution = 4096
9584
self.torqueEnabled = False
9685
self.moving = False
9786
self.presentPosition = 0
87+
self._rpm = 0.229
88+
self.bauds = {
89+
7: 4500000,
90+
6: 4000000,
91+
5: 3000000,
92+
4: 2000000,
93+
3: 1000000,
94+
2: 115200,
95+
1: 57600,
96+
0: 9600,
97+
}
9898

9999
async def run(self):
100100
while True:
@@ -113,3 +113,14 @@ def clear(self, position: bool = False, error: bool = False):
113113
if not isinstance(self.protocol, Protocol2):
114114
return
115115
self.protocol.clear(self.id, position=position, error=error)
116+
117+
@classmethod
118+
def convertToNegative(cls, value, length):
119+
if value < 0:
120+
maxInt = int.from_bytes(bytes([0xFF] * length), "little")
121+
maxInt += value
122+
return list(maxInt.to_bytes(length, "little"))
123+
124+
@classmethod
125+
def convertFromNegative(cls, value, length):
126+
return utils.twosComplement(value, length)

‎dynamixel/servo.py‎

Lines changed: 48 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -8,15 +8,26 @@
88
from dynamixel.protocol import Protocol1, Protocol2, Response
99

1010

11-
class paramUnit:
12-
UNIT_RAW = 0
13-
UNIT_PERCENT = 1
14-
UNIT_RPM = 2
15-
UNIT_DEGREE = 3
16-
UNIT_MILLI_AMPERE = 4
11+
class units:
12+
RAW = 0
13+
PERCENT = 1
14+
RPM = 2
15+
DEGREE = 3
16+
MILLI_AMPERE = 4
17+
VOLTAGE = 5
18+
BAUD = 6
1719

1820

19-
ControlTableItem = namedtuple("ControlTableItem", ["address", "length", "writable"])
21+
class ControlTableItem:
22+
def __init__(self, address, length, writable, limits=None, defaultUnit=units.RAW):
23+
self.address = address
24+
self.length = length
25+
self.writable = writable
26+
self.limits = limits
27+
self.defaultUnit = defaultUnit
28+
29+
def __iter__(self):
30+
yield from [self.address, self.length, self.writable, self.limits, self.defaultUnit]
2031

2132

2233
class controlTable:
@@ -30,20 +41,38 @@ def items(cls):
3041

3142
class Servo:
3243
CONTROL_TABLE = controlTable
44+
UNITS = units
3345

3446
def __init__(self, name: str, servo_id: int, **kwargs):
3547
self.name = name
3648
self._id = servo_id
3749
self.protocol: Protocol1 | Protocol2 = None
3850
self.resolution = None
51+
self.bauds = {}
52+
self._rpm = 1
3953
_ = kwargs
4054

4155
def convertUnits(self, raw: int, unit: int) -> int:
42-
unitMap = {paramUnit.UNIT_DEGREE: lambda raw: int((raw / 360) * self.resolution)}
56+
if unit == units.BAUD:
57+
inverseBauds = {v: k for k, v in self.bauds.items()}
58+
else:
59+
inverseBauds = {}
60+
61+
unitMap = {
62+
units.DEGREE: lambda raw: int((raw / 360) * self.resolution),
63+
units.VOLTAGE: lambda raw: int(raw * 10),
64+
units.BAUD: lambda raw: inverseBauds[raw],
65+
units.RPM: lambda raw: self._rpm * raw,
66+
}
4367
return unitMap.get(unit, lambda raw: raw)(raw)
4468

4569
def convertRaw(self, raw: int, unit: int) -> int:
46-
unitMap = {paramUnit.UNIT_DEGREE: lambda raw: int((raw / self.resolution) * 360)}
70+
unitMap = {
71+
units.DEGREE: lambda raw: int((raw / self.resolution) * 360),
72+
units.VOLTAGE: lambda raw: raw / 10,
73+
units.BAUD: lambda raw: self.bauds[raw],
74+
units.RPM: lambda raw: raw / self._rpm,
75+
}
4776
return unitMap.get(unit, lambda raw: raw)(raw)
4877

4978
def read(self, address: int, length: int) -> Response:
@@ -74,3 +103,13 @@ def writeControlTableItem(self, address, size, data) -> Response:
74103
def ping(self):
75104
res = self.protocol.ping(self.id)
76105
return res
106+
107+
@classmethod
108+
def convertToNegative(cls, value, length):
109+
_ = length
110+
return value
111+
112+
@classmethod
113+
def convertFromNegative(cls, value, length):
114+
_ = length
115+
return value

0 commit comments

Comments
 (0)