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
142
143
144
145
|
"""
api_layers/protocols/cml.py
CoolMuscle Language (CML) protocol layer.
For CoolMuscle CM-series servo motors over RS-232 (single axis)
or RS-485 (multi-drop, up to 31 axes).
Frame format
─────────────
RS-232 (address=0):
Command: <CMD>[<data>]\\r
Response: <CMD>[<data>]\\r (echo-back)
RS-485 (address 1-31):
Command: #<addr><CMD>[<data>]\\r
Response: *<addr><CMD>[<data>]\\r
Read commands (query current state):
TP — Tell absolute position (encoder counts, signed)
TV — Tell velocity (counts/second, signed)
TC — Tell current (% rated × 10, unsigned)
TS — Tell status word (hex flags)
Write commands (control):
ME — Motor Enable
MD — Motor Disable
MA<value> — Move Absolute (encoder counts)
MR<value> — Move Relative (encoder counts)
VS<value> — Velocity Setpoint
Channel IDs follow pattern: <motor_id>_<CMD>
e.g. "M1_TP", "M1_TV", "M2_TC"
"""
import math
import re
from dataclasses import dataclass, field
from typing import Dict, List, Optional
from api_layers.protocols.base_protocol import BaseProtocol
_READ_CMDS = ["TP", "TV", "TC", "TS"]
_RESP_RE = re.compile(r"[*@]?\d*([A-Z]{2})([\s\S]*)")
_NUM_RE = re.compile(r"[+-]?\d+")
@dataclass
class CMLMotor:
motor_id: str
address: int = 1 # 0 = RS-232 (no address prefix)
read_cmds: List[str] = field(default_factory=lambda: ["TP", "TV", "TC"])
class CMLLayer(BaseProtocol):
def __init__(
self,
port: str,
baud: int = 38400,
motors: List[CMLMotor] = None,
poll_interval: float = 0.1,
simulate: bool = True,
):
super().__init__(port, baud, poll_interval, simulate)
self.motors = motors or [CMLMotor("M1", address=1)]
# ── Protocol ──────────────────────────────────────────────────────────
def _send_query(self, motor: CMLMotor, cmd: str) -> Optional[float]:
if motor.address == 0:
frame = f"{cmd}\r".encode()
else:
frame = f"#{motor.address}{cmd}\r".encode()
with self._io_lock:
self._ser.write(frame)
self._ser.flush()
resp = self._ser.readline().decode(errors="replace").strip()
return _parse_cml_response(resp)
def _poll(self) -> Dict[str, float]:
result: Dict[str, float] = {}
for motor in self.motors:
for cmd in motor.read_cmds:
try:
val = self._send_query(motor, cmd)
if val is not None:
result[f"{motor.motor_id}_{cmd}"] = val
except Exception:
pass
return result
def _simulate(self, t: float) -> Dict[str, float]:
result: Dict[str, float] = {}
for i, motor in enumerate(self.motors):
phase = i * 1.0
for cmd in motor.read_cmds:
key = f"{motor.motor_id}_{cmd}"
if cmd == "TP":
val = math.sin(t * 0.2 + phase) * 10000.0
elif cmd == "TV":
val = math.cos(t * 0.2 + phase) * 2000.0
elif cmd == "TC":
val = abs(math.sin(t * 0.4 + phase)) * 500.0
elif cmd == "TS":
val = 0.0
else:
val = math.sin(t + i) * 100.0
result[key] = val
return result
def write(self, channel_id: str, value) -> bool:
# channel_id: "<motor_id>_<CMD>[<data>]"
# e.g. "M1_ME", "M1_MA" (value carries position)
parts = channel_id.split("_", 1)
if len(parts) != 2:
return False
motor_id, cmd = parts
motor = next((m for m in self.motors if m.motor_id == motor_id), None)
if motor is None:
return False
if self.simulate:
return True
try:
data = "" if cmd in ("ME", "MD") else str(int(value))
if motor.address == 0:
frame = f"{cmd}{data}\r".encode()
else:
frame = f"#{motor.address}{cmd}{data}\r".encode()
with self._io_lock:
self._ser.write(frame)
self._ser.flush()
self._ser.readline() # discard echo-back so it doesn't desync the next poll read
return True
except Exception:
return False
def _parse_cml_response(resp: str) -> Optional[float]:
"""Extract numeric value from CML response like '*1TP+001234'."""
m = _NUM_RE.search(resp[3:] if resp and resp[0] in "*@#" else resp)
if m:
return float(m.group())
return None
|
