diff options
Diffstat (limited to 'api_layers/protocols/cml.py')
| -rw-r--r-- | api_layers/protocols/cml.py | 118 |
1 files changed, 74 insertions, 44 deletions
diff --git a/api_layers/protocols/cml.py b/api_layers/protocols/cml.py index 6bc85f0..113afe7 100644 --- a/api_layers/protocols/cml.py +++ b/api_layers/protocols/cml.py @@ -1,36 +1,38 @@ """ api_layers/protocols/cml.py -CoolMuscle Language (CML) protocol layer. +CoolMuscle CM1-C ASCII (CML) protocol layer. -For CoolMuscle CM-series servo motors over RS-232 (single axis) -or RS-485 (multi-drop, up to 31 axes). +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) -Frame format -───────────── - RS-232 (address=0): - Command: <CMD>[<data>]\\r - Response: <CMD>[<data>]\\r (echo-back) +Motor ID is always appended explicitly as ".<id>" on every command — there is +no address-less / prefix-free mode. - RS-485 (address 1-31): - Command: #<addr><CMD>[<data>]\\r - Response: *<addr><CMD>[<data>]\\r +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 — Tell absolute position (encoder counts, signed) - TV — Tell velocity (counts/second, signed) - TC — Tell current (% rated × 10, unsigned) - TS — Tell status word (hex flags) + 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 - MA<value> — Move Absolute (encoder counts) - MR<value> — Move Relative (encoder counts) - VS<value> — Velocity Setpoint + 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_TV", "M2_TC" + e.g. "M1_TP", "M1_VS", "M2_TC" """ import math @@ -41,15 +43,19 @@ 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+") +_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 # 0 = RS-232 (no address prefix) + address: int = 1 # CM1-C motor ID, always sent as ".<address>" read_cmds: List[str] = field(default_factory=lambda: ["TP", "TV", "TC"]) @@ -66,17 +72,27 @@ class CMLLayer(BaseProtocol): 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]: - if motor.address == 0: - frame = f"{cmd}\r".encode() - else: - frame = f"#{motor.address}{cmd}\r".encode() + query = _QUERY_CMDS.get(cmd) + if query is None: + return None with self._io_lock: - self._ser.write(frame) - self._ser.flush() - resp = self._ser.readline().decode(errors="replace").strip() + self._send(motor.address, query) + resp = self._read_reply() return _parse_cml_response(resp) def _poll(self) -> Dict[str, float]: @@ -111,8 +127,7 @@ class CMLLayer(BaseProtocol): 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) + # 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 @@ -123,23 +138,38 @@ class CMLLayer(BaseProtocol): 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 + 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 numeric value from CML response like '*1TP+001234'.""" - m = _NUM_RE.search(resp[3:] if resp and resp[0] in "*@#" else resp) + """Extract the first numeric value from a CM1-C reply.""" + m = _NUM_RE.search(resp) if m: return float(m.group()) return None |