feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
|
||||
@@ -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."""
|
||||
@@ -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)
|
||||
@@ -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])]
|
||||
Reference in New Issue
Block a user