From 75e9bb8cf4cc0fa1b75ef79a2a7c7e9155dbfce3 Mon Sep 17 00:00:00 2001 From: seung6lee Date: Wed, 12 Aug 2026 13:15:11 +0900 Subject: [PATCH] us final version --- .env.example | 7 + .gitignore | 4 + arm.py | 147 ++++++++++++++ config.py | 39 ++++ main.py | 17 ++ planner.py | 91 +++++++++ presets.py | 61 ++++++ requirements.txt | 7 + ui.py | 502 +++++++++++++++++++++++++++++++++++++++++++++++ voice.py | 72 +++++++ 10 files changed, 947 insertions(+) create mode 100644 .env.example create mode 100644 .gitignore create mode 100644 arm.py create mode 100644 config.py create mode 100644 main.py create mode 100644 planner.py create mode 100644 presets.py create mode 100644 requirements.txt create mode 100644 ui.py create mode 100644 voice.py diff --git a/.env.example b/.env.example new file mode 100644 index 0000000..89c04a3 --- /dev/null +++ b/.env.example @@ -0,0 +1,7 @@ +OPENAI_API_KEY= +OPENAI_MODEL=gpt-5-mini +OPENAI_REASONING_EFFORT=low + +# en-US, ko-KR, ... +STT_LANGUAGE=en-US +BAUD_RATE=9600 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..69cac2e --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +.env +.venv/ +__pycache__/ +*.pyc diff --git a/arm.py b/arm.py new file mode 100644 index 0000000..021040f --- /dev/null +++ b/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)) diff --git a/config.py b/config.py new file mode 100644 index 0000000..498f84e --- /dev/null +++ b/config.py @@ -0,0 +1,39 @@ +import os +from dataclasses import dataclass +from pathlib import Path + +from dotenv import load_dotenv + +load_dotenv(Path(__file__).with_name(".env")) + + +@dataclass(frozen=True) +class Servo: + key: str + name: str + role: str + home: int + lo: int = 0 + hi: int = 180 + + +# Keep lo/hi in sync with LIMITS in robot_arm.ino +SERVOS = ( + Servo("s1", "base", "rotates the whole arm left/right; lower = right, higher = left", home=50), + Servo("s2", "shoulder", "lifts the arm forward; 0 = folded back, 120 = reaching far out", home=0), + Servo("s3", "elbow", "bends the forearm; 140 = tucked in, 0 = fully extended", home=140), + Servo("s4", "wrist", "tilts the tip up/down", home=10), +) + +HOME = tuple(s.home for s in SERVOS) + +OPENAI_API_KEY = os.getenv("OPENAI_API_KEY", "") +OPENAI_MODEL = os.getenv("OPENAI_MODEL", "gpt-5-mini") +OPENAI_REASONING_EFFORT = os.getenv("OPENAI_REASONING_EFFORT", "") + +STT_LANGUAGE = os.getenv("STT_LANGUAGE", "en-US") +BAUD_RATE = int(os.getenv("BAUD_RATE", "9600")) + + +def clamp_pose(pose): + return tuple(max(s.lo, min(s.hi, int(round(a)))) for s, a in zip(SERVOS, pose)) diff --git a/main.py b/main.py new file mode 100644 index 0000000..b71e02d --- /dev/null +++ b/main.py @@ -0,0 +1,17 @@ +import sys + +from PySide6.QtWidgets import QApplication + +from ui import MainWindow + + +def main(): + app = QApplication(sys.argv) + app.setApplicationName("Stage Control") + window = MainWindow() + window.show() + sys.exit(app.exec()) + + +if __name__ == "__main__": + main() diff --git a/planner.py b/planner.py new file mode 100644 index 0000000..35c1064 --- /dev/null +++ b/planner.py @@ -0,0 +1,91 @@ +from dataclasses import dataclass + +from openai import OpenAI +from pydantic import BaseModel + +import config +from config import SERVOS, HOME, clamp_pose +from presets import EXAMPLES + +MAX_STEPS = 16 +MIN_MS, MAX_MS = 80, 6000 + + +class Step(BaseModel): + s1: int + s2: int + s3: int + s4: int + ms: int + + +class Reply(BaseModel): + summary: str + steps: list[Step] + + +@dataclass +class Plan: + summary: str + steps: list # [(pose, ms), ...] + + +def _fmt(steps): + return "\n".join(f" {list(pose)} over {ms}ms" for pose, ms in steps) + + +def _instructions(): + servos = "\n".join( + f"- {s.key} ({s.name}): {s.role}. range {s.lo}-{s.hi}, home {s.home}" + for s in SERVOS + ) + examples = "\n\n".join(f'"{name}":\n{_fmt(steps)}' for name, steps in EXAMPLES.items()) + return f"""You drive a small 4-servo robot arm. Turn the user's spoken command into keyframes. + +Servos (angles in degrees): +{servos} + +Each step is an absolute pose [s1, s2, s3, s4] reached by moving all servos together over `ms` milliseconds. +Repeat the same pose with a duration to hold still. + +Rules: +- Stay inside each servo's range. +- {MIN_MS}-{MAX_MS} ms per step, at most {MAX_STEPS} steps. ~4 degrees per 30 ms looks natural; go faster only for snappy moves. +- Unless the user asks to hold a position, finish back at home {list(HOME)}. +- Relative commands ("a bit more left", "lower") start from the current pose. +- If the command has nothing to do with moving the arm, return no steps and say why in the summary. +- summary: a few words describing the motion, in the user's language. + +Reference moves: + +{examples}""" + + +class Planner: + def __init__(self): + if not config.OPENAI_API_KEY: + raise RuntimeError("OPENAI_API_KEY is not set") + self._client = OpenAI(api_key=config.OPENAI_API_KEY) + + def plan(self, text, current_pose): + kwargs = {} + if config.OPENAI_REASONING_EFFORT: + kwargs["reasoning"] = {"effort": config.OPENAI_REASONING_EFFORT} + + resp = self._client.responses.parse( + model=config.OPENAI_MODEL, + instructions=_instructions(), + input=f"Current pose: {list(current_pose)}\nCommand: {text}", + text_format=Reply, + timeout=30, + **kwargs, + ) + reply = resp.output_parsed + if reply is None: + raise RuntimeError("model returned nothing usable") + + steps = [ + (clamp_pose((s.s1, s.s2, s.s3, s.s4)), max(MIN_MS, min(MAX_MS, s.ms))) + for s in reply.steps[:MAX_STEPS] + ] + return Plan(reply.summary.strip(), steps) diff --git a/presets.py b/presets.py new file mode 100644 index 0000000..9fb21dd --- /dev/null +++ b/presets.py @@ -0,0 +1,61 @@ +from config import HOME + +# The three moves the old firmware had baked in, rewritten as keyframes. +# They double as examples for the LLM and as an offline fallback. + +PUSH = [ + ((50, 0, 0, 10), 300), + ((50, 120, 60, 10), 300), + ((50, 120, 60, 10), 2000), + (HOME, 1000), +] + +SPIN = [ + ((50, 80, 60, 60), 300), + ((180, 80, 60, 60), 1000), + ((0, 80, 60, 60), 1350), + ((180, 80, 60, 60), 1350), + ((0, 80, 60, 60), 1350), + ((180, 80, 60, 60), 1350), + ((50, 80, 60, 60), 1000), + ((50, 80, 60, 60), 2000), + (HOME, 1000), +] + +PENALTY = [ + ((50, 0, 0, 10), 300), + ((50, 0, 0, 10), 500), + ((50, 120, 0, 10), 150), + ((50, 120, 0, 10), 2000), + (HOME, 1000), +] + +GO_HOME = [(HOME, 1000)] + +EXAMPLES = { + "push": PUSH, + "spin around": SPIN, + "penalty kick": PENALTY, +} + +KEYWORDS = [ + (("push",), "push", PUSH), + (("spin", "spend"), "spin", SPIN), + (("penalty", "kick"), "penalty", PENALTY), + (("home", "reset", "처음", "홈"), "home", GO_HOME), +] + +STOP_WORDS = ("stop", "halt", "freeze", "정지", "멈춰", "스톱") + + +def is_stop(text): + t = text.lower() + return any(w in t for w in STOP_WORDS) + + +def match_keyword(text): + t = text.lower() + for words, label, steps in KEYWORDS: + if any(w in t for w in words): + return label, steps + return None diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..aa1460a --- /dev/null +++ b/requirements.txt @@ -0,0 +1,7 @@ +PySide6>=6.7 +pyserial>=3.5 +sounddevice>=0.4.7 +numpy>=1.26 +SpeechRecognition>=3.10 +openai>=1.60 +python-dotenv>=1.0 diff --git a/ui.py b/ui.py new file mode 100644 index 0000000..bb04464 --- /dev/null +++ b/ui.py @@ -0,0 +1,502 @@ +import html +import threading +from enum import Enum, auto + +from PySide6.QtCore import QObject, Qt, QTimer, Signal, QPropertyAnimation +from PySide6.QtGui import QFont, QKeySequence, QShortcut +from PySide6.QtWidgets import ( + QDialog, QFrame, QGraphicsOpacityEffect, QLabel, QLineEdit, QListWidget, + QListWidgetItem, QPushButton, QScrollArea, QVBoxLayout, QWidget, +) + +import config +import presets +import voice +from arm import Arm, ArmError, available_ports +from planner import Planner + +BG_DARK = "#0D0D0D" +BG_CARD = "#1A1A2E" +TEXT = "#FFFFFF" +LABEL = "#888888" +GREEN = "#00E676" +YELLOW = "#FFD600" +RED = "#FF5252" +BTN_CONNECT = "#1565C0" +BTN_DISCONNECT = "#B71C1C" +BTN_VOICE = "#6A1B9A" +DIVIDER = "#333333" + +STYLE = f""" +QWidget#root, QDialog {{ background: {BG_DARK}; }} +QFrame#card {{ background: {BG_CARD}; border-radius: 12px; }} +QLabel {{ color: {TEXT}; background: transparent; }} +QLabel#caption {{ color: {LABEL}; font-size: 14px; }} +QScrollArea, QScrollArea > QWidget > QWidget {{ background: transparent; border: none; }} +QPushButton {{ + color: white; border: none; border-radius: 6px; font-weight: bold; +}} +QPushButton:disabled {{ color: rgba(255, 255, 255, 0.4); }} +QLineEdit {{ + background: {BG_CARD}; color: {TEXT}; border: 1px solid {DIVIDER}; + border-radius: 8px; padding: 10px 12px; font-size: 15px; +}} +QLineEdit:focus {{ border-color: {BTN_VOICE}; }} +QLineEdit:disabled {{ color: {LABEL}; }} +QListWidget {{ + background: {BG_DARK}; color: {TEXT}; border: none; font-size: 20px; outline: none; +}} +QListWidget::item {{ + background: {BG_CARD}; padding: 16px; border-bottom: 1px solid {DIVIDER}; +}} +QListWidget::item:hover {{ background: #24243e; }} +QListWidget::item:selected {{ background: #2c2c4a; }} +""" + + +def _button_style(color, height, size): + r, g, b = (int(color[i:i + 2], 16) for i in (1, 3, 5)) + return f""" + QPushButton {{ background: {color}; min-height: {height}px; max-height: {height}px; font-size: {size}px; }} + QPushButton:hover {{ background: {color}; border: 2px solid rgba(255,255,255,0.15); }} + QPushButton:pressed {{ background: {color}; padding-top: 3px; }} + QPushButton:disabled {{ background: rgba({r},{g},{b},0.4); }} + """ + + +def _caption(text): + label = QLabel(text.upper()) + label.setObjectName("caption") + font = label.font() + font.setLetterSpacing(QFont.PercentageSpacing, 110) + label.setFont(font) + return label + + +def _card(): + frame = QFrame() + frame.setObjectName("card") + layout = QVBoxLayout(frame) + layout.setContentsMargins(20, 20, 20, 20) + layout.setSpacing(8) + return frame, layout + + +class BtState(Enum): + NOT_CONNECTED = auto() + CONNECTING = auto() + CONNECTED = auto() + + +class Busy(Enum): + IDLE = auto() + LISTENING = auto() + THINKING = auto() + MOVING = auto() + + +class Events(QObject): + connected = Signal(object, str) + connect_failed = Signal(str) + lost = Signal(str) + heard = Signal(str) + hear_failed = Signal(str) + planned = Signal(str, str, object) + plan_failed = Signal(str, str) + step = Signal(int, int, object) + done = Signal(bool) + + +class Toast(QLabel): + def __init__(self, parent): + super().__init__(parent) + self.setStyleSheet( + "background: rgba(50,50,50,0.95); color: white; border-radius: 18px;" + "padding: 10px 18px; font-size: 14px;" + ) + self.setAlignment(Qt.AlignCenter) + self.setWordWrap(True) + self._fx = QGraphicsOpacityEffect(self) + self.setGraphicsEffect(self._fx) + self._fade = QPropertyAnimation(self._fx, b"opacity", self) + self._fade.setDuration(300) + self._fade.finished.connect(lambda: self._fx.opacity() == 0 and self.hide()) + self._timer = QTimer(self, singleShot=True, timeout=self._out) + self.hide() + + def show_text(self, text, long=False): + self.setText(text) + width = min(self.parent().width() - 48, 360) + self.setFixedWidth(width) + self.adjustSize() + self.move((self.parent().width() - width) // 2, self.parent().height() - self.height() - 140) + self.raise_() + self.show() + self._fade.stop() + self._fx.setOpacity(1) + self._timer.start(3500 if long else 2000) + + def _out(self): + self._fade.setStartValue(1.0) + self._fade.setEndValue(0.0) + self._fade.start() + + +class DeviceDialog(QDialog): + def __init__(self, ports, parent=None): + super().__init__(parent) + self.setWindowTitle("Select Paired Device") + self.resize(parent.size()) + self.selected = None + + layout = QVBoxLayout(self) + layout.setContentsMargins(0, 0, 0, 0) + layout.setSpacing(0) + + header = QLabel("Select Paired Device") + header.setStyleSheet(f"background: {BG_CARD}; font-size: 24px; font-weight: bold; padding: 24px;") + hint = QLabel("Please select your HC-05 device") + hint.setStyleSheet(f"color: {LABEL}; font-size: 16px; padding: 16px;") + + self.list = QListWidget() + for label, device in ports: + item = QListWidgetItem(f"{label}\n{device}") + item.setData(Qt.UserRole, (label, device)) + self.list.addItem(item) + self.list.itemClicked.connect(self._pick) + self.list.itemActivated.connect(self._pick) + + layout.addWidget(header) + layout.addWidget(hint) + layout.addWidget(self.list, 1) + + def _pick(self, item): + self.selected = item.data(Qt.UserRole) + self.accept() + + +class MainWindow(QWidget): + def __init__(self): + super().__init__() + self.setObjectName("root") + self.setWindowTitle("Stage Control") + self.setStyleSheet(STYLE) + self.resize(440, 800) + self.setMinimumSize(360, 620) + + self.arm = None + self.device_name = "" + self.bt = BtState.NOT_CONNECTED + self.busy = Busy.IDLE + self._planner = None + + self.ev = Events() + self.ev.connected.connect(self._on_connected) + self.ev.connect_failed.connect(self._on_connect_failed) + self.ev.lost.connect(self._on_lost) + self.ev.heard.connect(self._on_heard) + self.ev.hear_failed.connect(self._on_hear_failed) + self.ev.planned.connect(self._on_planned) + self.ev.plan_failed.connect(self._on_plan_failed) + self.ev.step.connect(self._on_step) + self.ev.done.connect(self._on_done) + + self._build() + self.toast = Toast(self) + + QShortcut(QKeySequence(Qt.Key_Space), self, self._voice_shortcut) + QShortcut(QKeySequence(Qt.Key_Escape), self, self._stop) + + self._set_bt(BtState.NOT_CONNECTED) + if not config.OPENAI_API_KEY: + QTimer.singleShot(400, lambda: self.toast.show_text( + "OPENAI_API_KEY is not set.\nOnly preset keywords will work.", long=True)) + + def _build(self): + root = QVBoxLayout(self) + root.setContentsMargins(24, 24, 24, 24) + root.setSpacing(0) + + status_card, lay = _card() + lay.addWidget(_caption("Bluetooth Status")) + self.status = QLabel() + self.status.setStyleSheet("font-size: 26px; font-weight: bold;") + lay.addWidget(self.status) + root.addWidget(status_card) + root.addSpacing(16) + + voice_card, lay = _card() + lay.addWidget(_caption("Recognized commands")) + self.recognized = QLabel("—") + self.recognized.setWordWrap(True) + self.recognized.setTextFormat(Qt.RichText) + self.recognized.setAlignment(Qt.AlignLeft | Qt.AlignTop) + self.recognized.setTextInteractionFlags(Qt.TextSelectableByMouse) + self.recognized.setStyleSheet("font-size: 16px;") + scroll = QScrollArea() + scroll.setWidgetResizable(True) + scroll.setWidget(self.recognized) + lay.addWidget(scroll, 1) + root.addWidget(voice_card, 15) + root.addSpacing(16) + + sent_card, lay = _card() + lay.addWidget(_caption("Commands sent")) + self.sent = QLabel("—") + self.sent.setAlignment(Qt.AlignCenter) + self.sent.setStyleSheet(f"color: {GREEN}; font-size: 60px; font-weight: bold;") + self.sent_detail = QLabel("") + self.sent_detail.setAlignment(Qt.AlignCenter) + self.sent_detail.setStyleSheet(f"color: {LABEL}; font-size: 13px;") + lay.addStretch(1) + lay.addWidget(self.sent) + lay.addWidget(self.sent_detail) + lay.addStretch(1) + root.addWidget(sent_card, 10) + root.addSpacing(24) + + self.btn_connect = QPushButton() + self.btn_connect.setCursor(Qt.PointingHandCursor) + self.btn_connect.setFocusPolicy(Qt.NoFocus) + self.btn_connect.clicked.connect(self._connect_clicked) + root.addWidget(self.btn_connect) + root.addSpacing(16) + + self.btn_voice = QPushButton("Voice commands") + self.btn_voice.setCursor(Qt.PointingHandCursor) + self.btn_voice.setFocusPolicy(Qt.NoFocus) + self.btn_voice.setStyleSheet(_button_style(BTN_VOICE, 96, 28)) + self.btn_voice.clicked.connect(self._voice_clicked) + root.addWidget(self.btn_voice) + root.addSpacing(12) + + self.typed = QLineEdit() + self.typed.setPlaceholderText("…or type a command and press Enter") + self.typed.returnPressed.connect(self._typed_command) + root.addWidget(self.typed) + + # state + + def _set_bt(self, state): + self.bt = state + if state is BtState.NOT_CONNECTED: + self.status.setText("⚫ Not Connected") + self._status_color(RED) + self.btn_connect.setText("Connect Bluetooth") + self.btn_connect.setStyleSheet(_button_style(BTN_CONNECT, 72, 22)) + self.btn_connect.setEnabled(True) + elif state is BtState.CONNECTING: + self.status.setText("🟡 Connecting...") + self._status_color(YELLOW) + self.btn_connect.setText("Connecting...") + self.btn_connect.setEnabled(False) + else: + self.status.setText(f"🟢 Connected: {self.device_name}") + self._status_color(GREEN) + self.btn_connect.setText("Disconnect") + self.btn_connect.setStyleSheet(_button_style(BTN_DISCONNECT, 72, 22)) + self.btn_connect.setEnabled(True) + self._refresh_voice() + + def _set_busy(self, busy): + self.busy = busy + self._refresh_voice() + + def _refresh_voice(self): + connected = self.bt is BtState.CONNECTED + labels = { + Busy.IDLE: "Voice commands", + Busy.LISTENING: "Listening...", + Busy.THINKING: "Thinking...", + Busy.MOVING: "■ Stop", + } + self.btn_voice.setText(labels[self.busy]) + color = RED if self.busy is Busy.MOVING else BTN_VOICE + self.btn_voice.setStyleSheet(_button_style(color, 96, 28)) + self.btn_voice.setEnabled(connected and self.busy in (Busy.IDLE, Busy.MOVING)) + self.typed.setEnabled(connected and self.busy is Busy.IDLE) + + def _status_color(self, color): + self.status.setStyleSheet(f"font-size: 26px; font-weight: bold; color: {color};") + + def _show_recognized(self, text, note=None, note_color=LABEL): + body = html.escape(text) + if note: + body += f'
{html.escape(note)}' + self.recognized.setText(body) + + def _show_sent(self, big, detail=""): + size = 60 if len(big) <= 4 else 36 + self.sent.setStyleSheet(f"color: {GREEN}; font-size: {size}px; font-weight: bold;") + self.sent.setText(big) + self.sent_detail.setText(detail) + + # connect + + def _connect_clicked(self): + if self.bt is BtState.CONNECTED: + self._disconnect() + return + + ports = available_ports() + if not ports: + self.toast.show_text("No paired devices found.\nPair your HC-05 in system settings first.", long=True) + return + + dialog = DeviceDialog(ports, self) + if dialog.exec() != QDialog.Accepted or not dialog.selected: + return + + label, device = dialog.selected + self.device_name = label + self._set_bt(BtState.CONNECTING) + + def work(): + arm = Arm(device, on_lost=self.ev.lost.emit) + try: + arm.open() + except ArmError as e: + self.ev.connect_failed.emit(str(e)) + return + self.ev.connected.emit(arm, label) + + threading.Thread(target=work, daemon=True).start() + + def _on_connected(self, arm, label): + self.arm = arm + self.device_name = label + self._set_bt(BtState.CONNECTED) + + def _on_connect_failed(self, err): + self._set_bt(BtState.NOT_CONNECTED) + self.toast.show_text(f"Connection failed: {err}", long=True) + + def _on_lost(self, err): + self.arm = None + self._set_busy(Busy.IDLE) + self._set_bt(BtState.NOT_CONNECTED) + self.toast.show_text("Connection lost") + + def _disconnect(self): + if self.arm: + self.arm.close() + self.arm = None + self._set_busy(Busy.IDLE) + self._set_bt(BtState.NOT_CONNECTED) + + # voice + + def _voice_shortcut(self): + if self.btn_voice.isEnabled(): + self._voice_clicked() + + def _voice_clicked(self): + if self.busy is Busy.MOVING: + self._stop() + return + + self._set_busy(Busy.LISTENING) + self._show_recognized("🎙 Speak command") + + def work(): + try: + self.ev.heard.emit(voice.listen(config.STT_LANGUAGE)) + except voice.NothingHeard: + self.ev.hear_failed.emit("Didn't catch that. Try again.") + except Exception as e: + self.ev.hear_failed.emit(f"Voice recognition failed: {e}") + + threading.Thread(target=work, daemon=True).start() + + def _on_hear_failed(self, msg): + self._set_busy(Busy.IDLE) + self._show_recognized(msg) + + def _typed_command(self): + text = self.typed.text().strip() + if text and self.typed.isEnabled(): + self.typed.clear() + self._on_heard(text) + + def _on_heard(self, text): + if presets.is_stop(text): + self._show_recognized(text) + self._stop() + self._set_busy(Busy.IDLE) + return + + self._set_busy(Busy.THINKING) + self._show_recognized(f"Analyzing: {text}") + pose = self.arm.pose if self.arm else config.HOME + + def work(): + try: + if self._planner is None: + self._planner = Planner() + plan = self._planner.plan(text, pose) + self.ev.planned.emit(text, plan.summary, plan.steps) + except Exception as e: + self.ev.plan_failed.emit(text, str(e)) + + threading.Thread(target=work, daemon=True).start() + + def _on_planned(self, text, summary, steps): + if not steps: + self._set_busy(Busy.IDLE) + self._show_recognized(text, summary or "Nothing to do.") + return + self._show_recognized(text, f"→ {summary}") + self._run(steps) + + def _on_plan_failed(self, text, err): + print(f"[planner] {err}") + fallback = presets.match_keyword(text) + if fallback: + label, steps = fallback + self._show_recognized(text, f"Fallback: {label}", YELLOW) + self._run(steps) + else: + self._set_busy(Busy.IDLE) + self._show_recognized(text, f"LLM error: {err}", RED) + + # motion + + def _run(self, steps): + arm = self.arm + if arm is None: + self._set_busy(Busy.IDLE) + return + self._set_busy(Busy.MOVING) + total = len(steps) + + def work(): + try: + finished = arm.run(steps, lambda i, pose: self.ev.step.emit(i, total, pose)) + except ArmError: + finished = False + self.ev.done.emit(finished) + + threading.Thread(target=work, daemon=True).start() + + def _on_step(self, i, total, pose): + self._show_sent(" · ".join(map(str, pose)), f"step {i + 1} / {total}") + + def _on_done(self, finished): + if self.busy is Busy.MOVING: + self._set_busy(Busy.IDLE) + if not finished and self.arm: + self.sent_detail.setText("stopped") + + def _stop(self): + if not self.arm: + return + try: + self.arm.stop() + except ArmError: + return + self._show_sent("S", "stop") + + def closeEvent(self, event): + if self.arm: + self.arm.close() + super().closeEvent(event) diff --git a/voice.py b/voice.py new file mode 100644 index 0000000..48f63ac --- /dev/null +++ b/voice.py @@ -0,0 +1,72 @@ +import queue +from collections import deque + +import numpy as np +import sounddevice as sd +import speech_recognition as sr + +RATE = 16000 +CHUNK = RATE // 20 # 50 ms +CHUNK_S = CHUNK / RATE + + +class NothingHeard(Exception): + pass + + +def _rms(block): + return float(np.sqrt(np.mean(block.astype(np.float32) ** 2))) + + +def record(wait_for_speech=5.0, max_len=10.0, trailing_silence=0.9): + """Record from the default mic until the speaker pauses. Returns raw int16 PCM.""" + q = queue.Queue() + noise = [] + preroll = deque(maxlen=6) + frames = [] + quiet = elapsed = 0.0 + + def on_audio(data, frames_, time_, status): + q.put(data[:, 0].copy()) + + with sd.InputStream(samplerate=RATE, channels=1, dtype="int16", + blocksize=CHUNK, callback=on_audio): + while elapsed < max_len: + block = q.get(timeout=2) + elapsed += CHUNK_S + level = _rms(block) + + if len(noise) < 6: + noise.append(level) + preroll.append(block) + continue + threshold = max(float(np.median(noise)) * 2.5, 250.0) + + if not frames: + if level < threshold: + preroll.append(block) + if elapsed > wait_for_speech: + raise NothingHeard() + continue + frames.extend(preroll) + + frames.append(block) + quiet = quiet + CHUNK_S if level < threshold else 0.0 + if quiet > trailing_silence: + break + + if not frames: + raise NothingHeard() + return np.concatenate(frames).tobytes() + + +def transcribe(pcm, language): + audio = sr.AudioData(pcm, RATE, 2) + try: + return sr.Recognizer().recognize_google(audio, language=language) + except sr.UnknownValueError: + raise NothingHeard() + + +def listen(language): + return transcribe(record(), language)