148 lines
4.4 KiB
Python
148 lines
4.4 KiB
Python
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))
|