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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
"""
api_layers/protocols/cml.py
CoolMuscle CM1-C ASCII (CML) protocol layer.
Wire format — see docs/CM1-C_ASCII_Command_Cheatsheet.md:
<CMD>.<motor_id>[=<value>]\\r
e.g. "S0.1=20\\r" (set speed register), "?97.1\\r" (query speed),
"^.1\\r" (execute), "(.1\\r" (enable), ").1\\r" (disable), "].1\\r" (stop)
Motor ID is always appended explicitly as ".<id>" on every command — there is
no address-less / prefix-free mode.
Terminator is CR only (default K70 setting). Replies are read up to that CR
with Serial.read_until(b"\\r") rather than readline(), since pyserial's
readline() looks for LF by default and would otherwise block for the full
serial timeout on every single query.
Read commands (query current state):
TP — current position (?96, pulses, signed)
TV — current speed (?97, unit set by K37, signed)
TC — averaged current (?98, % rated, unsigned)
TS — motor status (?99, bit field — see cheatsheet)
Write commands (control):
ME — Motor Enable ("(")
MD — Motor Disable (")")
ST — Immediate stop ("]")
VS<value> — Velocity setpoint: sets a large P0 target (direction via sign
of S0) then executes with "^" — continuous rotation until
stopped (S0=0 or ST/MD).
MA<value> — Move absolute: sets P0 to <value> then executes with "^".
Channel IDs follow pattern: <motor_id>_<CMD>
e.g. "M1_TP", "M1_VS", "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
_QUERY_CMDS = {"TP": "?96", "TV": "?97", "TC": "?98", "TS": "?99"}
_NUM_RE = re.compile(r"[+-]?\d+\.?\d*")
# Position target for velocity-mode continuous rotation — direction comes
# from the sign of S0, not this value, so it just needs to be far enough
# away that the move never completes on its own.
_CONTINUOUS_POSITION = 1_000_000_000
@dataclass
class CMLMotor:
motor_id: str
address: int = 1 # CM1-C motor ID, always sent as ".<address>"
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)]
# ── Low-level I/O ─────────────────────────────────────────────────────
def _send(self, address: int, cmd: str, value: Optional[int] = None) -> None:
suffix = f"={value}" if value is not None else ""
frame = f"{cmd}.{address}{suffix}\r".encode()
self._ser.reset_input_buffer()
self._ser.write(frame)
self._ser.flush()
def _read_reply(self) -> str:
return self._ser.read_until(b"\r").decode(errors="replace").strip()
# ── Protocol ──────────────────────────────────────────────────────────
def _send_query(self, motor: CMLMotor, cmd: str) -> Optional[float]:
query = _QUERY_CMDS.get(cmd)
if query is None:
return None
with self._io_lock:
self._send(motor.address, query)
resp = self._read_reply()
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>" e.g. "M1_ME", "M1_VS" (value carries speed)
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:
with self._io_lock:
if cmd == "ME":
self._send(motor.address, "(")
self._read_reply()
elif cmd == "MD":
self._send(motor.address, ")")
self._read_reply()
elif cmd == "ST":
self._send(motor.address, "]")
self._read_reply()
elif cmd == "VS":
self._send(motor.address, "P0", _CONTINUOUS_POSITION)
self._read_reply()
self._send(motor.address, "S0", int(value))
self._read_reply()
self._send(motor.address, "^")
self._read_reply()
elif cmd == "MA":
self._send(motor.address, "P0", int(value))
self._read_reply()
self._send(motor.address, "^")
self._read_reply()
else:
return False
return True
except Exception:
return False
def _parse_cml_response(resp: str) -> Optional[float]:
"""Extract the first numeric value from a CM1-C reply."""
m = _NUM_RE.search(resp)
if m:
return float(m.group())
return None
|
