110 lines
3.9 KiB
Python
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])]
|