us final version
This commit is contained in:
@@ -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
|
||||
@@ -0,0 +1,4 @@
|
||||
.env
|
||||
.venv/
|
||||
__pycache__/
|
||||
*.pyc
|
||||
@@ -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))
|
||||
@@ -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))
|
||||
@@ -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()
|
||||
+91
@@ -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)
|
||||
+61
@@ -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
|
||||
@@ -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
|
||||
@@ -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'<br><span style="color:{note_color}; font-size:14px;">{html.escape(note)}</span>'
|
||||
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)
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user