83 lines
2.6 KiB
Python
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)
|