Skip to content

Commit 1da057d

Browse files
dbddbd
committed
working check
1 parent c2a17f8 commit 1da057d

6 files changed

Lines changed: 208 additions & 382 deletions

File tree

‎dynamixel/devices/__init__.py‎

Lines changed: 28 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,36 @@
55

66
# gnarly way to handle auto imports if you don't include all the devices in the lib
77
# when adding a new devicves add them to imports with (file_name, class_name) format
8+
from dynamixel.servo import ControlTableItem
89

910
imports = [("xl430w250t", "XL430_W250_T"), ("ax12a", "AX12A")]
11+
12+
13+
def make_getter(addr, size):
14+
def getter(self, _addr=addr, _size=size):
15+
return self.readControlTableItem(_addr, _size)
16+
17+
return getter
18+
19+
20+
def make_setter(addr, size):
21+
def setter(self, data, _addr=addr, _size=size):
22+
return self.writeControlTableItem(_addr, _size, data)
23+
24+
return setter
25+
26+
1027
for filename, classname in imports:
1128
mod = __import__(filename, None, None, (), 1)
12-
globals()[classname] = getattr(mod, classname)
29+
cls = getattr(mod, classname)
30+
globals()[classname] = cls
31+
for key, ct in cls.CONTROL_TABLE.items():
32+
if not isinstance(ct, ControlTableItem):
33+
continue
34+
baseName = "".join([word[0] + word[1:].lower() for word in key.split("_")])
35+
if ct.writable:
36+
methodName = f"set{baseName}"
37+
setattr(cls, methodName, make_setter(*ct[:2]))
1338

14-
del mod
15-
del filename
16-
del imports
17-
del classname
39+
methodName = f"get{baseName}"
40+
setattr(cls, methodName, make_getter(*ct[:2]))

‎dynamixel/devices/ax12a.py‎

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

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

1212

1313
class operatingMode:
1414
OP_JOINT = [0, 0]
1515
OP_WHEEL = [1, 1]
1616

1717

18-
class controlTable:
19-
MODEL_NUMBER = (0, 2)
20-
FIRMWARE_VERSION = (2, 1)
21-
ID = (3, 1)
22-
BAUD = (4, 1)
23-
RETURN_DELAY_TIME = (5, 1)
24-
CW_ANGLE_LIMIT = (6, 2)
25-
CCW_ANGLE_LIMIT = (8, 2)
26-
TEMPERATURE_LIMIT = (11, 1)
27-
MIN_VOLTAGE_LIMIT = (12, 1)
28-
MAX_VOLTAGE_LIMIT = (13, 1)
29-
MAX_TORQUE = (14, 2)
30-
STATUS_RETURN_LEVEL = (16, 1)
31-
ALARM_LED = (17, 1)
32-
SHUTDOWN = (18, 1)
33-
34-
TORQUE_ENABLE = (24, 1)
35-
LED = (25, 1)
36-
CW_COMPLIANCE_MARGIN = (26, 1)
37-
CCW_COMPLIANCE_MARGIN = (27, 1)
38-
CW_COMPLIANCE_SLOPE = (28, 1)
39-
CCW_COMPLIANCE_SLOPE = (29, 1)
40-
GOAL_POSITION = (30, 2)
41-
MOVING_SPEED = (32, 2)
42-
TORQUE_LIMIT = (34, 2)
43-
PRESENT_POSITION = (36, 2)
44-
PRESENT_SPEED = (38, 2)
45-
PRESENT_LOAD = (40, 2)
46-
PRESENT_VOLTAGE = (42, 1)
47-
PRESENT_TEMPERATURE = (43, 1)
48-
REGISTERED = (44, 1)
49-
MOVING = (46, 1)
50-
LOCK = (47, 1)
51-
PUNCH = (48, 2)
18+
class ControlTable(controlTable):
19+
MODEL_NUMBER = ControlTableItem(0, 2, False)
20+
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)
31+
ALARM_LED = ControlTableItem(17, 1, True)
32+
SHUTDOWN = ControlTableItem(18, 1, True)
33+
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)
44+
PRESENT_SPEED = ControlTableItem(38, 2, False)
45+
PRESENT_LOAD = ControlTableItem(40, 2, False)
46+
PRESENT_VOLTAGE = ControlTableItem(42, 1, False)
47+
PRESENT_TEMPERATURE = ControlTableItem(43, 1, False)
48+
REGISTERED = ControlTableItem(44, 1, False)
49+
MOVING = ControlTableItem(46, 1, False)
50+
LOCK = ControlTableItem(47, 1, True)
51+
PUNCH = ControlTableItem(48, 2, True)
5252

5353

5454
class AX12A(Servo):
55-
CONTROL_TABLE = controlTable
55+
CONTROL_TABLE = ControlTable
5656
PARAM_UNIT = paramUnit
5757
OPERATING_MODE = operatingMode
5858

@@ -77,89 +77,3 @@ async def run(self):
7777
if (res := self.readControlTableItem(self.CONTROL_TABLE.TORQUE_ENABLE)).ok:
7878
self.torqueEnabled = bool(res.data)
7979
await asyncio.sleep(0.1)
80-
81-
def ping(self):
82-
res = self.protocol.ping(self.id)
83-
return res
84-
85-
def torqueOn(self) -> Response:
86-
return self.writeControlTableItem(self.CONTROL_TABLE.TORQUE_ENABLE, 1)
87-
88-
def torqueOff(self):
89-
return self.writeControlTableItem(self.CONTROL_TABLE.TORQUE_ENABLE, 0)
90-
91-
def getBaud(self):
92-
res = self.readControlTableItem(self.CONTROL_TABLE.BAUD)
93-
return res
94-
95-
def getModelNumber(self):
96-
res = self.readControlTableItem(self.CONTROL_TABLE.BAUD)
97-
return res
98-
99-
def setBaudrate(self, baud: int):
100-
return self.writeControlTableItem(self.CONTROL_TABLE.BAUD, baud)
101-
102-
def ledOn(self):
103-
return self.writeControlTableItem(self.CONTROL_TABLE.LED, 1)
104-
105-
def ledOff(self):
106-
return self.writeControlTableItem(self.CONTROL_TABLE.LED, 0)
107-
108-
def setOperationMode(self, operatingMode):
109-
self.writeControlTableItem(self.CONTROL_TABLE.CW_ANGLE_LIMIT, operatingMode[0])
110-
return self.writeControlTableItem(self.CONTROL_TABLE.CCW_ANGLE_LIMIT, operatingMode[1])
111-
112-
def convertToNegative(self, value, length):
113-
if value < 0:
114-
maxInt = int.from_bytes(bytes([0xFF] * length), "little")
115-
maxInt += value
116-
return list(maxInt.to_bytes(length, "little"))
117-
118-
def convertFromNegative(self, value, length):
119-
return utils.twosComplement(value, length)
120-
121-
def setGoalPosition(self, value, unit=None):
122-
# Need to do a unit conversion
123-
unit = unit or self.unit
124-
value = self.convertUnits(value, unit)
125-
if value < 0:
126-
value = self.convertToNegative(value, self.CONTROL_TABLE.GOAL_POSITION[1])
127-
return self.writeControlTableItem(self.CONTROL_TABLE.GOAL_POSITION, value)
128-
129-
def getPresentPosition(self, unit=None):
130-
unit = unit or self.unit
131-
res = self.readControlTableItem(self.CONTROL_TABLE.PRESENT_POSITION)
132-
data = None
133-
if res.ok:
134-
data = self.convertFromNegative(res.data, self.CONTROL_TABLE.GOAL_POSITION[1])
135-
data = self.convertRaw(data, unit)
136-
return Response(data, res.err)
137-
138-
def setMaxPosition(self, value, unit=None):
139-
unit = unit or self.unit
140-
value = self.convertUnits(value, unit)
141-
return self.writeControlTableItem(self.CONTROL_TABLE.CW_ANGLE_LIMIT, value)
142-
143-
def setMinPosition(self, value, unit=None):
144-
unit = unit or self.unit
145-
value = self.convertUnits(value, unit)
146-
return self.writeControlTableItem(self.CONTROL_TABLE.CCW_ANGLE_LIMIT, value)
147-
148-
def getPositionLimits(self, unit=None):
149-
unit = unit or self.unit
150-
maxRes = self.readControlTableItem(self.CONTROL_TABLE.CW_ANGLE_LIMIT)
151-
minRes = self.readControlTableItem(self.CONTROL_TABLE.CCW_ANGLE_LIMIT)
152-
if isinstance(maxRes, int):
153-
maxRes = self.convertRaw(maxRes, unit)
154-
if isinstance(minRes, int):
155-
minRes = self.convertRaw(minRes, unit)
156-
return (minRes, maxRes)
157-
158-
def setGoalVelocity(self, value):
159-
return self.writeControlTableItem(self.CONTROL_TABLE.MOVING_SPEED, value)
160-
161-
def getPresentVelocity(self):
162-
return self.readControlTableItem(self.CONTROL_TABLE.PRESENT_SPEED)
163-
164-
def getTorqueEnabled(self):
165-
return self.readControlTableItem(self.CONTROL_TABLE.TORQUE_ENABLE)

0 commit comments

Comments
 (0)