feat: add Microduck UNO Q deployment and hotspot skill control

This commit is contained in:
2026-11-19 13:49:08 +08:00
parent 2b0c395508
commit 164a397726
99 changed files with 12488 additions and 4 deletions
+1
View File
@@ -0,0 +1 @@
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
+57
View File
@@ -0,0 +1,57 @@
from __future__ import annotations
from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from typing import Any
@dataclass
class HardwareFrame:
gyro_raw: list[float]
proj_gravity_raw: list[float]
ticks: list[int]
raw_speeds: list[int]
voltage_v: float = 7.4
max_temp_c: float = 35.0
imu_ok: bool = True
servo_err_mask: int = 0
timestamp_s: float = 0.0
extra: dict[str, Any] = field(default_factory=dict)
class HardwareBridge(ABC):
name: str = "abstract"
@abstractmethod
def connect(self) -> None:
"""Initialize connection to the MCU / servo bus."""
@abstractmethod
def close(self) -> None:
"""Release hardware resources safely."""
@abstractmethod
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
"""Push servo IDs, I2C address, watchdog, and IMU bias config to HAL."""
@abstractmethod
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
"""Send target joint ticks and read fresh IMU + servo feedback."""
@abstractmethod
def set_torque(self, enable: bool) -> None:
"""Immediately enable or disable torque on all configured servos."""
@abstractmethod
def ping_servos(self, max_id: int = 20) -> list[int]:
"""Scan and return responding servo IDs."""
@abstractmethod
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
"""Collect stationary gyro readings (rad/s in sensor frame) and return mean bias."""
+82
View File
@@ -0,0 +1,82 @@
from __future__ import annotations
import math
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class MockHal(HardwareBridge):
"""PC-runnable hardware simulator for pre-hardware verification."""
name = "mock"
def __init__(self, initial_tick: int = 2048) -> None:
self.connected = False
self.torque_enabled = False
self.servo_ids = list(range(1, 15))
self.ticks = [int(initial_tick)] * 14
self.raw_speeds = [0] * 14
self._gyro = [0.0, 0.0, 0.0]
self._gravity = [0.0, 0.0, -1.0]
self._alpha = 0.45
def connect(self) -> None:
self.connected = True
def close(self) -> None:
self.torque_enabled = False
self.connected = False
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
joints = profile.get("joints", {})
self.servo_ids = [int(joints[n]["id"]) for n in joint_names]
def set_mock_gravity(self, grav: list[float]) -> None:
gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2])
norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0
self._gravity = [gx / norm, gy / norm, gz / norm]
def set_mock_gyro(self, gyro: list[float]) -> None:
self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])]
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
self.torque_enabled = bool(enable_torque)
if self.torque_enabled:
for i in range(14):
prev = self.ticks[i]
nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev)))
delta = nxt - prev
self.ticks[i] = max(0, min(4095, nxt))
mag = min(0x7FFF, abs(delta) * 50)
self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag
else:
self.raw_speeds = [0] * 14
return HardwareFrame(
gyro_raw=list(self._gyro),
proj_gravity_raw=list(self._gravity),
ticks=list(self.ticks),
raw_speeds=list(self.raw_speeds),
voltage_v=7.8,
max_temp_c=34.0,
imu_ok=True,
servo_err_mask=0,
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
self.torque_enabled = bool(enable)
def ping_servos(self, max_id: int = 20) -> list[int]:
return [sid for sid in self.servo_ids if 0 <= sid <= max_id]
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
return list(self._gyro)
+109
View File
@@ -0,0 +1,109 @@
from __future__ import annotations
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class RouterBridgeHal(HardwareBridge):
"""Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge."""
name = "router"
def __init__(self) -> None:
self._bridge = None
self._servo_ids = list(range(1, 15))
def connect(self) -> None:
try:
from arduino.app_utils import Bridge # type: ignore
except ImportError as exc:
raise RuntimeError(
"arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / "
"arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging."
) from exc
self._bridge = Bridge
def close(self) -> None:
if self._bridge is not None:
try:
self.set_torque(False)
except Exception:
pass
self._bridge = None
def _require_bridge(self) -> Any:
if self._bridge is None:
raise RuntimeError("RouterBridgeHal is not connected")
return self._bridge
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
bridge = self._require_bridge()
joints = profile["joints"]
servo = profile["servo"]
imu = profile["imu"]
safety = profile.get("safety", {})
self._servo_ids = [int(joints[name]["id"]) for name in joint_names]
addr_raw = imu.get("i2c_addr", "0x6A")
i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw)
bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])]
payload: list[float] = [
float(servo.get("baudrate", 1000000)),
float(i2c_addr),
float(safety.get("watchdog_ms", 150)),
bias[0],
bias[1],
bias[2],
*[float(sid) for sid in self._servo_ids],
]
bridge.call("hw_configure", payload)
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
bridge = self._require_bridge()
if len(target_ticks) != 14:
raise ValueError("Expected 14 target_ticks")
cmd_frame = [
1 if enable_torque else 0,
int(speed_limit),
int(acc),
*[int(t) for t in target_ticks],
]
res = bridge.call("hw_sync_step", cmd_frame)
if not isinstance(res, (list, tuple)) or len(res) < 38:
raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}")
return HardwareFrame(
gyro_raw=[float(res[0]), float(res[1]), float(res[2])],
proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])],
ticks=[int(round(res[6 + i])) for i in range(14)],
raw_speeds=[int(round(res[20 + i])) for i in range(14)],
voltage_v=float(res[34]),
max_temp_c=float(res[35]),
imu_ok=bool(int(round(res[36]))),
servo_err_mask=int(round(res[37])),
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
bridge = self._require_bridge()
bridge.call("hw_set_torque", 1 if enable else 0)
def ping_servos(self, max_id: int = 20) -> list[int]:
bridge = self._require_bridge()
res = bridge.call("hw_ping_servos", int(max_id))
return [int(x) for x in res] if isinstance(res, (list, tuple)) else []
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
bridge = self._require_bridge()
res = bridge.call("hw_calibrate_imu", int(samples))
if not isinstance(res, (list, tuple)) or len(res) != 3:
raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}")
return [float(res[0]), float(res[1]), float(res[2])]