Added difference serial protocols

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