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)