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