diff options
Diffstat (limited to 'api_layers/protocols/cml.py')
| -rw-r--r-- | api_layers/protocols/cml.py | 139 |
1 files changed, 139 insertions, 0 deletions
diff --git a/api_layers/protocols/cml.py b/api_layers/protocols/cml.py new file mode 100644 index 0000000..dbee2fa --- /dev/null +++ b/api_layers/protocols/cml.py @@ -0,0 +1,139 @@ +""" +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() + 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 + 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() + self._ser.write(frame) + 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 |