feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -0,0 +1,82 @@
|
||||
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)
|
||||
Reference in New Issue
Block a user