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

83 lines
2.6 KiB
Python

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)