us final version

arm.py

@@ -0,0 +1,147 @@
+import queue
+import threading
+import time
+
+import serial
+from serial.tools import list_ports
+
+from config import BAUD_RATE, HOME
+
+_JUNK = ("bluetooth-incoming-port", "debug-console", "wlan-debug")
+_BT_HINTS = ("hc-05", "hc-06", "bluetooth", "rfcomm", "bt")
+
+
+def available_ports():
+    """Paired Bluetooth serial ports first, then whatever else is plugged in."""
+    ports = []
+    for p in list_ports.comports():
+        name = p.device.lower()
+        if any(j in name for j in _JUNK):
+            continue
+        label = p.description if p.description and p.description != "n/a" else p.device.split("/")[-1]
+        is_bt = any(h in (name + label.lower()) for h in _BT_HINTS)
+        ports.append((not is_bt, label, p.device))
+    return [(label, dev) for _, label, dev in sorted(ports)]
+
+
+class ArmError(Exception):
+    pass
+
+
+class Arm:
+    def __init__(self, port, on_lost=None):
+        self.port = port
+        self.pose = HOME
+        self._on_lost = on_lost
+        self._ser = None
+        self._replies = queue.Queue()
+        self._write_lock = threading.Lock()
+        self._stopping = threading.Event()
+        self._closed = False
+        self._live = False
+
+    def open(self):
+        try:
+            self._ser = serial.Serial(self.port, BAUD_RATE, timeout=0.2, write_timeout=2)
+        except (serial.SerialException, OSError) as e:
+            raise ArmError(str(e)) from None
+
+        threading.Thread(target=self._read_loop, daemon=True).start()
+
+        # USB boards reboot when the port opens, so keep pinging for a bit.
+        deadline = time.monotonic() + 4
+        while time.monotonic() < deadline:
+            self._send("P")
+            try:
+                if self._wait(("PONG",), 0.4):
+                    self._live = True
+                    return
+            except ArmError:
+                pass
+        self.close()
+        raise ArmError("no answer from the arm. Is robot_arm.ino uploaded?")
+
+    def close(self):
+        self._closed = True
+        if self._ser:
+            try:
+                self._ser.close()
+            except Exception:
+                pass
+
+    def move(self, pose, ms):
+        """Blocks until the arm gets there. Returns False if it was stopped midway."""
+        self._drain()
+        self._send("M " + " ".join(map(str, pose)) + f" {ms}")
+        reply = self._wait(("OK", "STOPPED"), ms / 1000 + 3)
+        if reply is None:
+            raise ArmError("arm stopped responding")
+        return reply == "OK"
+
+    def run(self, steps, on_step=None):
+        self._stopping.clear()
+        for i, (pose, ms) in enumerate(steps):
+            if self._stopping.is_set():
+                return False
+            if on_step:
+                on_step(i, pose)
+            if not self.move(pose, ms):
+                return False
+        return True
+
+    def stop(self):
+        self._stopping.set()
+        self._send("S")
+
+    def _send(self, line):
+        with self._write_lock:
+            try:
+                self._ser.write((line + "\n").encode())
+            except (serial.SerialException, OSError) as e:
+                self._lost(e)
+                raise ArmError("send failed") from None
+
+    def _drain(self):
+        while not self._replies.empty():
+            self._replies.get_nowait()
+
+    def _wait(self, kinds, timeout):
+        end = time.monotonic() + timeout
+        while (left := end - time.monotonic()) > 0:
+            try:
+                kind = self._replies.get(timeout=left)
+            except queue.Empty:
+                break
+            if kind in kinds:
+                return kind
+        return None
+
+    def _read_loop(self):
+        buf = b""
+        while not self._closed:
+            try:
+                buf += self._ser.read(64)
+            except (serial.SerialException, OSError, TypeError) as e:
+                self._lost(e)
+                return
+            while b"\n" in buf:
+                raw, buf = buf.split(b"\n", 1)
+                self._handle(raw.decode(errors="ignore").strip())
+
+    def _handle(self, line):
+        if not line:
+            return
+        kind, *rest = line.split()
+        if kind in ("OK", "STOPPED") and len(rest) == 4:
+            try:
+                self.pose = tuple(int(x) for x in rest)
+            except ValueError:
+                pass
+        self._replies.put(kind)
+
+    def _lost(self, err):
+        if self._closed:
+            return
+        self.close()
+        if self._live and self._on_lost:
+            self._on_lost(str(err))