Files
VibemakerSandbox/hardware/unoq/python/hal/router_hal.py
T

110 lines
3.9 KiB
Python

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])]