diff --git a/firmware/esp32_robot_idf/main/uuids.h b/firmware/esp32_robot_idf/main/uuids.h index 02f3a0dd..b351e4c5 100644 --- a/firmware/esp32_robot_idf/main/uuids.h +++ b/firmware/esp32_robot_idf/main/uuids.h @@ -1,4 +1,4 @@ -// Generated by tools/gen-uuids.py from protocol/uuids.json β€” do not edit. +// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. // Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. #pragma once @@ -23,6 +23,9 @@ #define FLASH_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3" #define PIN_CONFIG_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4" #define SIGNAL_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" +#define MOTION_GOAL_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6" +#define MOTION_POSE_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7" +#define MOTION_STATUS_CHAR_UUID "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8" // heartbeat_service #define HEARTBEAT_SVC_UUID "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0" diff --git a/firmware/pi_robot/motion_control.py b/firmware/pi_robot/motion_control.py new file mode 100644 index 00000000..4385b326 --- /dev/null +++ b/firmware/pi_robot/motion_control.py @@ -0,0 +1,186 @@ +"""Motion control for differential drive robots. + +Mirrors the interface of packages/control (C++). When that library is compiled +and importable as `control`, swap the import below; until then this pure-Python +implementation runs on the Pi unchanged. +""" + +import math + + +def _normalize_angle(a: float) -> float: + a = math.fmod(a + math.pi, 2.0 * math.pi) + if a < 0.0: + a += 2.0 * math.pi + return a - math.pi + + +class Pose2D: + def __init__(self, x: float = 0.0, y: float = 0.0, theta: float = 0.0): + self.x = x + self.y = y + self.theta = theta + + +class ControlOutput: + def __init__(self, v: float = 0.0, omega: float = 0.0): + self.v = v # linear velocity (m/s) + self.omega = omega # angular velocity (rad/s) + + +class WheelSpeeds: + def __init__(self, left: float = 0.0, right: float = 0.0): + self.left = left # [-100, 100] + self.right = right # [-100, 100] + + +class DiffDriveConfig: + def __init__(self, + wheel_separation: float = 0.15, + wheel_radius: float = 0.033, + max_wheel_speed: float = 0.5): + self.wheel_separation = wheel_separation # meters, center to center + self.wheel_radius = wheel_radius # meters + self.max_wheel_speed = max_wheel_speed # m/s + + +def to_wheel_speeds(output: ControlOutput, config: DiffDriveConfig) -> WheelSpeeds: + v_left = output.v - output.omega * config.wheel_separation / 2.0 + v_right = output.v + output.omega * config.wheel_separation / 2.0 + scale = 100.0 / config.max_wheel_speed if config.max_wheel_speed > 0.0 else 0.0 + return WheelSpeeds( + max(-100.0, min(100.0, v_left * scale)), + max(-100.0, min(100.0, v_right * scale)), + ) + + +class ContinuousController: + """Single continuous control law: v = k_rho·ρ·cos(Ξ±), Ο‰ = k_alphaΒ·Ξ± + k_betaΒ·Ξ². + Switches to heading-only spin when within dist_threshold of goal.""" + + def __init__(self, + k_rho: float = 0.3, + k_alpha: float = 0.8, + k_beta: float = -0.15, + k_theta: float = 0.5, + dist_threshold: float = 0.05, + dist_tolerance: float = 0.02, + angle_tolerance: float = 0.05, + max_v: float = 0.5, + max_omega: float = 1.5): + self.k_rho = k_rho + self.k_alpha = k_alpha + self.k_beta = k_beta + self.k_theta = k_theta + self.dist_threshold = dist_threshold + self.dist_tolerance = dist_tolerance + self.angle_tolerance = angle_tolerance + self.max_v = max_v + self.max_omega = max_omega + self._goal: Pose2D | None = None + self._active = False + self._done = False + + def setGoal(self, goal: Pose2D) -> None: + self._goal = goal + self._active = True + self._done = False + + def isDone(self) -> bool: + return self._done + + def cancel(self) -> None: + self._done = True + self._active = False + + def tick(self, current: Pose2D) -> ControlOutput: + if not self._active or self._done or self._goal is None: + return ControlOutput() + dx = self._goal.x - current.x + dy = self._goal.y - current.y + rho = math.sqrt(dx * dx + dy * dy) + if rho < self.dist_threshold: + heading_error = _normalize_angle(self._goal.theta - current.theta) + if abs(heading_error) < self.angle_tolerance: + self._done = True + self._active = False + return ControlOutput() + omega = max(-self.max_omega, min(self.max_omega, self.k_theta * heading_error)) + return ControlOutput(0.0, omega) + phi = math.atan2(dy, dx) + alpha = _normalize_angle(phi - current.theta) + beta = _normalize_angle(self._goal.theta - phi) + v = max(-self.max_v, min(self.max_v, self.k_rho * rho * math.cos(alpha))) + omega = max(-self.max_omega, min(self.max_omega, self.k_alpha * alpha + self.k_beta * beta)) + return ControlOutput(v, omega) + + +class SpinMoveSpinController: + """Three-phase controller: rotate to face goal, drive forward, rotate to final heading.""" + + _SPIN_TO_GOAL = "spin_to_goal" + _DRIVE = "drive" + _SPIN_TO_HEADING = "spin_to_heading" + _DONE = "done" + + def __init__(self, + k_spin: float = 0.8, + k_drive: float = 0.4, + k_heading: float = 0.3, + angle_tolerance: float = 0.05, + dist_tolerance: float = 0.02, + max_v: float = 0.5, + max_omega: float = 1.5): + self.k_spin = k_spin + self.k_drive = k_drive + self.k_heading = k_heading + self.angle_tolerance = angle_tolerance + self.dist_tolerance = dist_tolerance + self.max_v = max_v + self.max_omega = max_omega + self._goal: Pose2D | None = None + self._phase = self._DONE + self._active = False + + def setGoal(self, goal: Pose2D) -> None: + self._goal = goal + self._phase = self._SPIN_TO_GOAL + self._active = True + + def isDone(self) -> bool: + return self._phase == self._DONE + + def cancel(self) -> None: + self._phase = self._DONE + self._active = False + + def tick(self, current: Pose2D) -> ControlOutput: + if not self._active or self._phase == self._DONE or self._goal is None: + return ControlOutput() + dx = self._goal.x - current.x + dy = self._goal.y - current.y + rho = math.sqrt(dx * dx + dy * dy) + if self._phase == self._SPIN_TO_GOAL: + alpha = _normalize_angle(math.atan2(dy, dx) - current.theta) + if abs(alpha) < self.angle_tolerance: + self._phase = self._DRIVE + return ControlOutput() + omega = max(-self.max_omega, min(self.max_omega, self.k_spin * alpha)) + return ControlOutput(0.0, omega) + if self._phase == self._DRIVE: + if rho < self.dist_tolerance: + self._phase = self._SPIN_TO_HEADING + return ControlOutput() + heading_error = _normalize_angle(math.atan2(dy, dx) - current.theta) + v = max(-self.max_v, min(self.max_v, self.k_drive * rho)) + omega = max(-self.max_omega, min(self.max_omega, self.k_heading * heading_error)) + return ControlOutput(v, omega) + if self._phase == self._SPIN_TO_HEADING: + heading_error = _normalize_angle(self._goal.theta - current.theta) + if abs(heading_error) < self.angle_tolerance: + self._phase = self._DONE + self._active = False + return ControlOutput() + omega = max(-self.max_omega, min(self.max_omega, self.k_spin * heading_error)) + return ControlOutput(0.0, omega) + return ControlOutput() diff --git a/firmware/pi_robot/pi_robot.py b/firmware/pi_robot/pi_robot.py index 45d403d6..1652fa0d 100644 --- a/firmware/pi_robot/pi_robot.py +++ b/firmware/pi_robot/pi_robot.py @@ -62,6 +62,9 @@ OPS_RESPONSE_CHAR_UUID, TELEMETRY_CHAR_UUID, SIGNAL_CHAR_UUID, + MOTION_GOAL_CHAR_UUID, + MOTION_POSE_CHAR_UUID, + MOTION_STATUS_CHAR_UUID, ) # Capability schema, built at startup from config. Types name a UI/data @@ -80,6 +83,8 @@ def _build_caps() -> list: if MOTORS_ENABLED: caps.append({"name": "motors", "type": "signed-pair", "range": [-100, 100], "pins": MOTORS_PINS, "pin_mode": "pwm"}) + if MOTORS_ENABLED and _motion_available: + caps.append({"name": "motion", "type": "pose-control"}) caps.append({"name": "wifi", "type": "wifi-scan"}) caps.append({"name": "ota", "type": "bundle-ota"}) if CAMERA_ENABLED is not False: @@ -173,7 +178,10 @@ def _load_config() -> dict: ORIENT_SWAP = bool(_ORIENT.get("swap", False)) ORIENT_INVERT_A = bool(_ORIENT.get("invert_a", False)) ORIENT_INVERT_B = bool(_ORIENT.get("invert_b", False)) -CAMERA_ENABLED = _config.get("camera_enabled", "auto") # "auto" | True | False +CAMERA_ENABLED = _config.get("camera_enabled", "auto") # "auto" | True | False +WHEEL_SEPARATION = float(_config.get("wheel_separation", 0.15)) # meters, center to center +WHEEL_RADIUS = float(_config.get("wheel_radius", 0.033)) # meters +MAX_WHEEL_SPEED = float(_config.get("max_wheel_speed", 0.5)) # m/s # Dashboards the robot trusts. Each .pub is one line: # ssh-ed25519 [comment] @@ -256,6 +264,16 @@ def _fw_info_snapshot() -> dict: except Exception as _e: # noqa: BLE001 β€” see comment above _camera_import_err = f"{type(_e).__name__}: {_e}" +_motion_available = False +try: + from motion_control import ( + ContinuousController, SpinMoveSpinController, + DiffDriveConfig, to_wheel_speeds, Pose2D as MotionPose2D, + ) + _motion_available = True +except ImportError as _e: + _motion_available = False + logging.basicConfig(format="%(asctime)s %(message)s", level=logging.INFO) log = logging.getLogger("pi_robot") @@ -322,6 +340,11 @@ def _pin_conflicts() -> list[tuple[int, list[str]]]: # scheduled stop and the joystick's command wins. _motor_pulse_id: int = 0 +_motion_status: dict = {"st": "idle"} +_motion_current_pose = None # MotionPose2D | None β€” updated by dashboard +_motion_controller = None # ContinuousController | SpinMoveSpinController | None +_motion_task: asyncio.Task | None = None + _robot_status: dict = {"st": "ready"} _cam_pc = None @@ -832,6 +855,8 @@ def _apply_motors(left: int, right: int) -> None: def _motor_handle_write(data: bytearray) -> None: + if _motion_controller is not None: + _motion_cancel() """Motor char accepts two payload shapes: 2 bytes [l, r] β€” persistent (user joystick). Watchdog stops after MOTOR_WATCHDOG_MS silence. @@ -848,7 +873,20 @@ def signed(b: int) -> int: if len(data) == 2: _motor_last_write_at = asyncio.get_event_loop().time() _motor_pulse_id += 1 - _apply_motors(signed(data[0]), signed(data[1])) + forward_pct = signed(data[0]) # % of max speed, positive = forward + turn_pct = signed(data[1]) # % of max turn, positive = right + if _motion_available and MOTORS_ENABLED and WHEEL_SEPARATION > 0 and MAX_WHEEL_SPEED > 0: + max_omega = 2.0 * MAX_WHEEL_SPEED / WHEEL_SEPARATION + v = (forward_pct / 100.0) * MAX_WHEEL_SPEED + omega = -(turn_pct / 100.0) * max_omega # negative omega = clockwise = right + half_sep = WHEEL_SEPARATION / 2.0 + scale = 100.0 / MAX_WHEEL_SPEED + left = max(-100, min(100, int(round((v - omega * half_sep) * scale)))) + right = max(-100, min(100, int(round((v + omega * half_sep) * scale)))) + else: + left = max(-100, min(100, forward_pct + turn_pct)) + right = max(-100, min(100, forward_pct - turn_pct)) + _apply_motors(left, right) elif len(data) == 4: l = max(-LLM_MAX_SPEED, min(LLM_MAX_SPEED, signed(data[0]))) r = max(-LLM_MAX_SPEED, min(LLM_MAX_SPEED, signed(data[1]))) @@ -1354,6 +1392,114 @@ async def _motor_watchdog_task() -> None: log.info("motor watchdog: stopped") +# ── Motion controller ────────────────────────────────────────────────────── +# +# Dashboard writes the robot's current pose to MOTION_POSE_CHAR_UUID (from +# whatever localization source it has β€” April tags, odometry, etc.) and a +# goal pose + mode to MOTION_GOAL_CHAR_UUID. A 20 Hz asyncio task calls +# controller.tick() and forwards (v, Ο‰) β†’ (left, right) through _apply_motors. +# A manual write to MOTOR_CHAR_UUID cancels any active goal. + +def _motion_publish_status(st: str) -> None: + global _motion_status + _motion_status = {"st": st} + _publish(MOTION_STATUS_CHAR_UUID, _json_bytes(_motion_status)) + log.info("motion-status β†’ %s", st) + + +def _motion_cancel() -> None: + global _motion_controller, _motion_task + if _motion_controller is not None: + _motion_controller.cancel() + _motion_controller = None + if _motion_task is not None and not _motion_task.done(): + _motion_task.cancel() + _motion_task = None + _apply_motors(0, 0) + _motion_publish_status("cancelled") + + +async def _motion_tick_loop(cfg) -> None: + global _motion_controller + try: + while _motion_controller is not None: + if _motion_current_pose is not None: + output = _motion_controller.tick(_motion_current_pose) + wheels = to_wheel_speeds(output, cfg) + _apply_motors(int(round(wheels.left)), int(round(wheels.right))) + if _motion_controller is not None and _motion_controller.isDone(): + _motion_controller = None + _apply_motors(0, 0) + _motion_publish_status("done") + return + await asyncio.sleep(0.05) + except asyncio.CancelledError: + _apply_motors(0, 0) + raise + + +async def _motion_start(x: float, y: float, theta: float, mode: str, + wheel_sep: float = None, wheel_r: float = None, + max_spd: float = None) -> None: + global _motion_controller, _motion_task + if _motion_controller is not None: + _motion_controller.cancel() + _motion_controller = None + if _motion_task is not None and not _motion_task.done(): + _motion_task.cancel() + _motion_task = None + sep = wheel_sep if (wheel_sep and wheel_sep > 0) else WHEEL_SEPARATION + r = wheel_r if (wheel_r and wheel_r > 0) else WHEEL_RADIUS + spd = max_spd if (max_spd and max_spd > 0) else MAX_WHEEL_SPEED + cfg = DiffDriveConfig(sep, r, spd) + goal = MotionPose2D(x, y, theta) + ctrl = ContinuousController() if mode == "continuous" else SpinMoveSpinController() + ctrl.setGoal(goal) + _motion_controller = ctrl + _motion_task = asyncio.create_task(_motion_tick_loop(cfg)) + _motion_publish_status("moving") + log.info("motion: goal=(%.3f, %.3f, %.3f) mode=%s sep=%.3f r=%.3f spd=%.2f", + x, y, theta, mode, sep, r, spd) + + +def _motion_goal_handle_write(data: bytearray) -> None: + if not _motion_available or not MOTORS_ENABLED: + return + try: + msg = json.loads(bytes(data).decode("utf-8")) + except Exception as e: + log.warning("motion-goal: bad JSON β€” %s", e) + return + if msg.get("op") == "cancel": + _motion_cancel() + return + try: + x = float(msg["x"]) + y = float(msg["y"]) + theta = float(msg["theta"]) + except (KeyError, ValueError) as e: + log.warning("motion-goal: missing field β€” %s", e) + return + mode = str(msg.get("mode", "spin_move_spin")) + wheel_sep = float(msg["wheel_sep"]) if "wheel_sep" in msg else None + wheel_r = float(msg["wheel_r"]) if "wheel_r" in msg else None + max_spd = float(msg["max_spd"]) if "max_spd" in msg else None + _schedule(_motion_start(x, y, theta, mode, wheel_sep, wheel_r, max_spd)) + + +def _motion_pose_handle_write(data: bytearray) -> None: + global _motion_current_pose + if not _motion_available: + return + try: + msg = json.loads(bytes(data).decode("utf-8")) + _motion_current_pose = MotionPose2D( + float(msg["x"]), float(msg["y"]), float(msg["theta"]) + ) + except Exception as e: + log.warning("motion-pose: bad JSON β€” %s", e) + + async def _check_current_wifi() -> None: """On startup, reflect the actual current connection state.""" try: @@ -1434,6 +1580,8 @@ def on_read(characteristic: BlessGATTCharacteristic, **_) -> bytearray: # Chunked protocol β€” direct reads have no single value. Status # lands via notify on state changes. return bytearray() + if uuid == MOTION_STATUS_CHAR_UUID: + return _json_bytes(_motion_status) return characteristic.value @@ -1476,6 +1624,12 @@ def on_write(characteristic: BlessGATTCharacteristic, value: bytearray, **_) -> if uuid == SIGNAL_CHAR_UUID: _signal_handle_write(bytes(value)) return + if uuid == MOTION_GOAL_CHAR_UUID: + _motion_goal_handle_write(value) + return + if uuid == MOTION_POSE_CHAR_UUID: + _motion_pose_handle_write(value) + return def _init_wifi_radio() -> None: @@ -1624,6 +1778,30 @@ async def main() -> None: GATTAttributePermissions.readable, ) + if MOTORS_ENABLED and _motion_available: + await _server.add_new_characteristic( + SERVICE_UUID, MOTION_GOAL_CHAR_UUID, + GATTCharacteristicProperties.write, + bytearray(), + GATTAttributePermissions.writeable, + ) + await _server.add_new_characteristic( + SERVICE_UUID, MOTION_POSE_CHAR_UUID, + GATTCharacteristicProperties.write, + bytearray(), + GATTAttributePermissions.writeable, + ) + await _server.add_new_characteristic( + SERVICE_UUID, MOTION_STATUS_CHAR_UUID, + GATTCharacteristicProperties.read | GATTCharacteristicProperties.notify, + _json_bytes(_motion_status), + GATTAttributePermissions.readable, + ) + log.info("motion: ready (wheel_sep=%.3f wheel_r=%.3f max_spd=%.2f)", + WHEEL_SEPARATION, WHEEL_RADIUS, MAX_WHEEL_SPEED) + elif not _motion_available: + log.info("motion: unavailable (motion_control import failed)") + await _server.start() log.info("Advertising on service %s", SERVICE_UUID) log.info("Ctrl+C to stop.") @@ -1631,7 +1809,9 @@ async def main() -> None: asyncio.create_task(_telemetry_task()) if MOTORS_ENABLED: asyncio.create_task(_motor_watchdog_task()) - log.info("capabilities: led=%s motors=%s camera=%s", LED_ENABLED, MOTORS_ENABLED, _camera_available) + log.info("capabilities: led=%s motors=%s camera=%s motion=%s", + LED_ENABLED, MOTORS_ENABLED, _camera_available, + _motion_available and MOTORS_ENABLED) try: await asyncio.Event().wait() finally: diff --git a/firmware/pi_robot/uuids.py b/firmware/pi_robot/uuids.py index 9e53f6b8..16816641 100644 --- a/firmware/pi_robot/uuids.py +++ b/firmware/pi_robot/uuids.py @@ -1,4 +1,4 @@ -# Generated by tools/gen-uuids.py from protocol/uuids.json β€” do not edit. +# Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. # Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. # main_service @@ -22,6 +22,9 @@ FLASH_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3" PIN_CONFIG_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4" SIGNAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" +MOTION_GOAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6" +MOTION_POSE_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7" +MOTION_STATUS_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8" # heartbeat_service HEARTBEAT_SVC_UUID = "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0" diff --git a/packages/control/CMakeLists.txt b/packages/control/CMakeLists.txt new file mode 100644 index 00000000..d9ebff37 --- /dev/null +++ b/packages/control/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(control VERSION 0.1.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CONTROL_SOURCES + src/diff_drive.cpp + src/continuous_controller.cpp + src/spin_move_spin_controller.cpp +) + +add_library(control ${CONTROL_SOURCES}) +target_include_directories(control PUBLIC + $ + $ +) + +# Python extension β€” built only when pybind11 is available. +# On the Pi: pip install pybind11, then cmake -DBUILD_PYTHON=ON .. +option(BUILD_PYTHON "Build pybind11 Python extension" OFF) +if(BUILD_PYTHON) + find_package(pybind11 REQUIRED) + pybind11_add_module(control_py python/bindings.cpp ${CONTROL_SOURCES}) + target_include_directories(control_py PRIVATE include) + set_target_properties(control_py PROPERTIES OUTPUT_NAME "control") +endif() diff --git a/packages/control/include/control/continuous_controller.hpp b/packages/control/include/control/continuous_controller.hpp new file mode 100644 index 00000000..d110d408 --- /dev/null +++ b/packages/control/include/control/continuous_controller.hpp @@ -0,0 +1,30 @@ +#pragma once +#include "pose_controller.hpp" + +struct ContinuousParams { + double k_rho = 0.3; + double k_alpha = 0.8; + double k_beta = -0.15; + double k_theta = 0.5; + double dist_threshold = 0.05; // meters β€” switch to heading-only below this + double dist_tolerance = 0.02; // meters + double angle_tolerance = 0.05; // radians + double max_v = 0.5; // m/s + double max_omega = 1.5; // rad/s +}; + +class ContinuousController : public PoseController { +public: + explicit ContinuousController(ContinuousParams params = {}); + + void setGoal(Pose2D goal) override; + ControlOutput tick(Pose2D current) override; + bool isDone() const override; + void cancel() override; + +private: + ContinuousParams params_; + Pose2D goal_{}; + bool active_ = false; + bool done_ = false; +}; diff --git a/packages/control/include/control/control_output.hpp b/packages/control/include/control/control_output.hpp new file mode 100644 index 00000000..961400af --- /dev/null +++ b/packages/control/include/control/control_output.hpp @@ -0,0 +1,6 @@ +#pragma once + +struct ControlOutput { + double v; // linear velocity (m/s) + double omega; // angular velocity (rad/s) +}; diff --git a/packages/control/include/control/diff_drive.hpp b/packages/control/include/control/diff_drive.hpp new file mode 100644 index 00000000..196d7707 --- /dev/null +++ b/packages/control/include/control/diff_drive.hpp @@ -0,0 +1,15 @@ +#pragma once +#include "control_output.hpp" + +struct DiffDriveConfig { + double wheel_separation; // meters, center to center + double wheel_radius; // meters + double max_wheel_speed; // m/s +}; + +struct WheelSpeeds { + double left; // [-100, 100] + double right; // [-100, 100] +}; + +WheelSpeeds to_wheel_speeds(ControlOutput output, const DiffDriveConfig& config); diff --git a/packages/control/include/control/pose2d.hpp b/packages/control/include/control/pose2d.hpp new file mode 100644 index 00000000..e6d0f55a --- /dev/null +++ b/packages/control/include/control/pose2d.hpp @@ -0,0 +1,14 @@ +#pragma once +#include + +struct Pose2D { + double x; + double y; + double theta; +}; + +inline double normalize_angle(double a) { + a = std::fmod(a + M_PI, 2.0 * M_PI); + if (a < 0.0) a += 2.0 * M_PI; + return a - M_PI; +} diff --git a/packages/control/include/control/pose_controller.hpp b/packages/control/include/control/pose_controller.hpp new file mode 100644 index 00000000..50d9ba77 --- /dev/null +++ b/packages/control/include/control/pose_controller.hpp @@ -0,0 +1,12 @@ +#pragma once +#include "pose2d.hpp" +#include "control_output.hpp" + +class PoseController { +public: + virtual ~PoseController() = default; + virtual void setGoal(Pose2D goal) = 0; + virtual ControlOutput tick(Pose2D current) = 0; + virtual bool isDone() const = 0; + virtual void cancel() = 0; +}; diff --git a/packages/control/include/control/spin_move_spin_controller.hpp b/packages/control/include/control/spin_move_spin_controller.hpp new file mode 100644 index 00000000..f9c46973 --- /dev/null +++ b/packages/control/include/control/spin_move_spin_controller.hpp @@ -0,0 +1,30 @@ +#pragma once +#include "pose_controller.hpp" + +struct SpinMoveSpinParams { + double k_spin = 0.8; + double k_drive = 0.4; + double k_heading = 0.3; // drift correction during drive phase + double angle_tolerance = 0.05; // radians + double dist_tolerance = 0.02; // meters + double max_v = 0.5; // m/s + double max_omega = 1.5; // rad/s +}; + +class SpinMoveSpinController : public PoseController { +public: + explicit SpinMoveSpinController(SpinMoveSpinParams params = {}); + + void setGoal(Pose2D goal) override; + ControlOutput tick(Pose2D current) override; + bool isDone() const override; + void cancel() override; + +private: + enum class Phase { SPIN_TO_GOAL, DRIVE, SPIN_TO_HEADING, DONE }; + + SpinMoveSpinParams params_; + Pose2D goal_{}; + Phase phase_ = Phase::DONE; + bool active_ = false; +}; diff --git a/packages/control/python/bindings.cpp b/packages/control/python/bindings.cpp new file mode 100644 index 00000000..7e3ad623 --- /dev/null +++ b/packages/control/python/bindings.cpp @@ -0,0 +1,54 @@ +#include +#include "control/pose2d.hpp" +#include "control/control_output.hpp" +#include "control/diff_drive.hpp" +#include "control/continuous_controller.hpp" +#include "control/spin_move_spin_controller.hpp" + +namespace py = pybind11; + +PYBIND11_MODULE(control, m) { + py::class_(m, "Pose2D") + .def(py::init(), + py::arg("x") = 0.0, py::arg("y") = 0.0, py::arg("theta") = 0.0) + .def_readwrite("x", &Pose2D::x) + .def_readwrite("y", &Pose2D::y) + .def_readwrite("theta", &Pose2D::theta); + + py::class_(m, "ControlOutput") + .def(py::init(), + py::arg("v") = 0.0, py::arg("omega") = 0.0) + .def_readwrite("v", &ControlOutput::v) + .def_readwrite("omega", &ControlOutput::omega); + + py::class_(m, "WheelSpeeds") + .def(py::init(), + py::arg("left") = 0.0, py::arg("right") = 0.0) + .def_readwrite("left", &WheelSpeeds::left) + .def_readwrite("right", &WheelSpeeds::right); + + py::class_(m, "DiffDriveConfig") + .def(py::init(), + py::arg("wheel_separation") = 0.15, + py::arg("wheel_radius") = 0.033, + py::arg("max_wheel_speed") = 0.5) + .def_readwrite("wheel_separation", &DiffDriveConfig::wheel_separation) + .def_readwrite("wheel_radius", &DiffDriveConfig::wheel_radius) + .def_readwrite("max_wheel_speed", &DiffDriveConfig::max_wheel_speed); + + m.def("to_wheel_speeds", &to_wheel_speeds, py::arg("output"), py::arg("config")); + + py::class_(m, "ContinuousController") + .def(py::init(), py::arg("params") = ContinuousParams{}) + .def("setGoal", &ContinuousController::setGoal) + .def("tick", &ContinuousController::tick) + .def("isDone", &ContinuousController::isDone) + .def("cancel", &ContinuousController::cancel); + + py::class_(m, "SpinMoveSpinController") + .def(py::init(), py::arg("params") = SpinMoveSpinParams{}) + .def("setGoal", &SpinMoveSpinController::setGoal) + .def("tick", &SpinMoveSpinController::tick) + .def("isDone", &SpinMoveSpinController::isDone) + .def("cancel", &SpinMoveSpinController::cancel); +} diff --git a/packages/control/src/continuous_controller.cpp b/packages/control/src/continuous_controller.cpp new file mode 100644 index 00000000..a13857b2 --- /dev/null +++ b/packages/control/src/continuous_controller.cpp @@ -0,0 +1,50 @@ +#include "control/continuous_controller.hpp" +#include +#include + +ContinuousController::ContinuousController(ContinuousParams params) + : params_(params) {} + +void ContinuousController::setGoal(Pose2D goal) { + goal_ = goal; + active_ = true; + done_ = false; +} + +bool ContinuousController::isDone() const { return done_; } + +void ContinuousController::cancel() { + done_ = true; + active_ = false; +} + +ControlOutput ContinuousController::tick(Pose2D current) { + if (!active_ || done_) return {0.0, 0.0}; + + double dx = goal_.x - current.x; + double dy = goal_.y - current.y; + double rho = std::sqrt(dx * dx + dy * dy); + + if (rho < params_.dist_threshold) { + double heading_error = normalize_angle(goal_.theta - current.theta); + if (std::abs(heading_error) < params_.angle_tolerance) { + done_ = true; + active_ = false; + return {0.0, 0.0}; + } + double omega = std::clamp(params_.k_theta * heading_error, + -params_.max_omega, params_.max_omega); + return {0.0, omega}; + } + + double phi = std::atan2(dy, dx); + double alpha = normalize_angle(phi - current.theta); + double beta = normalize_angle(goal_.theta - phi); + + double v = std::clamp(params_.k_rho * rho * std::cos(alpha), + -params_.max_v, params_.max_v); + double omega = std::clamp(params_.k_alpha * alpha + params_.k_beta * beta, + -params_.max_omega, params_.max_omega); + + return {v, omega}; +} diff --git a/packages/control/src/diff_drive.cpp b/packages/control/src/diff_drive.cpp new file mode 100644 index 00000000..7a960e9c --- /dev/null +++ b/packages/control/src/diff_drive.cpp @@ -0,0 +1,14 @@ +#include "control/diff_drive.hpp" +#include +#include + +WheelSpeeds to_wheel_speeds(ControlOutput output, const DiffDriveConfig& config) { + double v_left = output.v - output.omega * config.wheel_separation / 2.0; + double v_right = output.v + output.omega * config.wheel_separation / 2.0; + + double scale = config.max_wheel_speed > 0.0 ? 100.0 / config.max_wheel_speed : 0.0; + return { + std::clamp(v_left * scale, -100.0, 100.0), + std::clamp(v_right * scale, -100.0, 100.0), + }; +} diff --git a/packages/control/src/spin_move_spin_controller.cpp b/packages/control/src/spin_move_spin_controller.cpp new file mode 100644 index 00000000..d2f78503 --- /dev/null +++ b/packages/control/src/spin_move_spin_controller.cpp @@ -0,0 +1,66 @@ +#include "control/spin_move_spin_controller.hpp" +#include +#include + +SpinMoveSpinController::SpinMoveSpinController(SpinMoveSpinParams params) + : params_(params) {} + +void SpinMoveSpinController::setGoal(Pose2D goal) { + goal_ = goal; + phase_ = Phase::SPIN_TO_GOAL; + active_ = true; +} + +bool SpinMoveSpinController::isDone() const { + return phase_ == Phase::DONE; +} + +void SpinMoveSpinController::cancel() { + phase_ = Phase::DONE; + active_ = false; +} + +ControlOutput SpinMoveSpinController::tick(Pose2D current) { + if (!active_ || phase_ == Phase::DONE) return {0.0, 0.0}; + + double dx = goal_.x - current.x; + double dy = goal_.y - current.y; + double rho = std::sqrt(dx * dx + dy * dy); + + switch (phase_) { + case Phase::SPIN_TO_GOAL: { + double alpha = normalize_angle(std::atan2(dy, dx) - current.theta); + if (std::abs(alpha) < params_.angle_tolerance) { + phase_ = Phase::DRIVE; + return {0.0, 0.0}; + } + return {0.0, std::clamp(params_.k_spin * alpha, + -params_.max_omega, params_.max_omega)}; + } + case Phase::DRIVE: { + if (rho < params_.dist_tolerance) { + phase_ = Phase::SPIN_TO_HEADING; + return {0.0, 0.0}; + } + double heading_error = normalize_angle(std::atan2(dy, dx) - current.theta); + double v = std::clamp(params_.k_drive * rho, + -params_.max_v, params_.max_v); + double omega = std::clamp(params_.k_heading * heading_error, + -params_.max_omega, params_.max_omega); + return {v, omega}; + } + case Phase::SPIN_TO_HEADING: { + double heading_error = normalize_angle(goal_.theta - current.theta); + if (std::abs(heading_error) < params_.angle_tolerance) { + phase_ = Phase::DONE; + active_ = false; + return {0.0, 0.0}; + } + return {0.0, std::clamp(params_.k_spin * heading_error, + -params_.max_omega, params_.max_omega)}; + } + case Phase::DONE: + break; + } + return {0.0, 0.0}; +} diff --git a/protocol/uuids.json b/protocol/uuids.json index 292d5a99..5583552f 100644 --- a/protocol/uuids.json +++ b/protocol/uuids.json @@ -20,7 +20,10 @@ "SNAPSHOT_DATA_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da1", "FLASH_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3", "PIN_CONFIG_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4", - "SIGNAL_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5" + "SIGNAL_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5", + "MOTION_GOAL_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6", + "MOTION_POSE_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7", + "MOTION_STATUS_CHAR_UUID": "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8" }, "heartbeat_service": { "HEARTBEAT_SVC_UUID": "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0", diff --git a/public/ble.js b/public/ble.js index c44e7e9c..86183801 100644 --- a/public/ble.js +++ b/public/ble.js @@ -10,6 +10,7 @@ import { CAMERA_SIGNAL_CHAR_UUID, CAMERA_STATUS_CHAR_UUID, OPS_CHAR_UUID, SNAPSHOT_REQUEST_CHAR_UUID, SNAPSHOT_DATA_CHAR_UUID, + MOTION_GOAL_CHAR_UUID, MOTION_POSE_CHAR_UUID, MOTION_STATUS_CHAR_UUID, } from "./uuids.js"; // Chunked-frame protocol shared by OTA + camera signaling: begin carries @@ -27,6 +28,7 @@ export const UUIDS_BY_CAP = { camera: { signal: CAMERA_SIGNAL_CHAR_UUID, status: CAMERA_STATUS_CHAR_UUID }, ops: OPS_CHAR_UUID, snapshot: { request: SNAPSHOT_REQUEST_CHAR_UUID, data: SNAPSHOT_DATA_CHAR_UUID }, + motion: { goal: MOTION_GOAL_CHAR_UUID, pose: MOTION_POSE_CHAR_UUID, status: MOTION_STATUS_CHAR_UUID }, }; export const decodeJson = (dv) => { diff --git a/public/capabilities/runtime/cap-section.js b/public/capabilities/runtime/cap-section.js index f87966a6..4741adcc 100644 --- a/public/capabilities/runtime/cap-section.js +++ b/public/capabilities/runtime/cap-section.js @@ -12,7 +12,7 @@ import { escapeHtml } from "../../dom.js"; // v3 β€” bumped when the default-open shape changed: motors now defaults // to open (it's the daily-driver verb); other caps stay collapsed. const STORE_KEY = "better-robotics:cap-open:v3"; -const DEFAULTS = { motors: true }; +const DEFAULTS = { motors: true, motion: true }; let _state = null; function load() { diff --git a/public/capabilities/runtime/index.js b/public/capabilities/runtime/index.js index d6b04055..fb908510 100644 --- a/public/capabilities/runtime/index.js +++ b/public/capabilities/runtime/index.js @@ -7,6 +7,7 @@ import { makeWifiScanCap } from "./wifi-scan.js"; import { makeWebrtcInstallableCap } from "./webrtc-installable.js"; import { makeMjpegStreamCap } from "./mjpeg-stream.js"; import { makeBleSnapshotCap } from "./ble-snapshot.js"; +import { makeMotionCap } from "./motion.js"; import { setRender as setBusRender } from "./render-bus.js"; export const RUNTIMES = { @@ -18,6 +19,7 @@ export const RUNTIMES = { "webrtc-installable": makeWebrtcInstallableCap, "mjpeg-stream": makeMjpegStreamCap, "ble-snapshot": makeBleSnapshotCap, + "pose-control": makeMotionCap, }; // All runtime caps share render-bus.js for the back-channel to the diff --git a/public/capabilities/runtime/motion.js b/public/capabilities/runtime/motion.js new file mode 100644 index 00000000..84d4363c --- /dev/null +++ b/public/capabilities/runtime/motion.js @@ -0,0 +1,275 @@ +// Motion-control cap: manual joypad + x/y/theta pose targeting over BLE. +// Schema: { name: "motion", type: "pose-control" } +// Three chars: goal (write JSON), pose (write JSON), status (notify JSON). +// Manual mode drives motorsChar directly so the Pi's diff-drive kinematics +// apply the same way as the dedicated motors card. +// +// CV correction: whenever entry.cvPosition is updated by the overhead camera +// panel, it is automatically written to motionPoseChar so the robot's +// odometry stays aligned with camera ground truth. No user action needed. + +import { UUIDS_BY_CAP, encodeJson, decodeJson } from "../../ble.js"; +import { attachJoypad } from "../../joypad.js"; +import { setPairValue } from "./signed-pair.js"; +import { logFor } from "../../log.js"; +import { capSection } from "./cap-section.js"; +import { renderEntry } from "./render-bus.js"; + +const LS_PREFIX = "better-robotics:motion"; +const CV_STALE_MS = 10_000; +const CV_POLL_MS = 500; + +function lsGet(entry, k, def = "") { + try { + const v = localStorage.getItem(`${LS_PREFIX}:${k}:${entry.name}`); + return v != null ? v : def; + } catch { return def; } +} +function lsSet(entry, k, v) { + try { localStorage.setItem(`${LS_PREFIX}:${k}:${entry.name}`, String(v)); } catch {} +} + +// Poll entry.cvPosition every CV_POLL_MS. When a new fix arrives (updatedAt +// has advanced) and it isn't stale, push it to motionPoseChar. Stored on the +// entry so cleanup() can cancel it on disconnect. +function startCvWatch(entry) { + stopCvWatch(entry); + let lastApplied = 0; + entry._cvWatchInterval = setInterval(async () => { + const p = entry.cvPosition; + if (!p || !entry.motionPoseChar) return; + if (p.updatedAt <= lastApplied) return; + if ((Date.now() - p.updatedAt) > CV_STALE_MS) return; + try { + await entry.motionPoseChar.writeValueWithResponse(encodeJson({ + x: p.x / 1000, + y: p.y / 1000, + theta: p.headingDeg * (Math.PI / 180), + })); + lastApplied = p.updatedAt; + logFor(entry, `motion cv fix: (${p.x}, ${p.y} mm, ${p.headingDeg}Β°)`); + } catch (err) { + logFor(entry, `motion cv fix failed: ${err.message}`); + } + }, CV_POLL_MS); +} + +function stopCvWatch(entry) { + if (entry._cvWatchInterval) { + clearInterval(entry._cvWatchInterval); + entry._cvWatchInterval = null; + } +} + +export function makeMotionCap(schema) { + const { name } = schema; + + return { + name, + schema, + + initEntry: () => ({ + motionGoalChar: null, + motionPoseChar: null, + motionStatusChar: null, + motionStatus: "idle", + _motionJoypad: null, + _cvWatchInterval: null, + }), + + async probe(entry, service) { + const uuids = UUIDS_BY_CAP.motion; + try { + entry.motionGoalChar = await service.getCharacteristic(uuids.goal); + entry.motionPoseChar = await service.getCharacteristic(uuids.pose); + entry.motionStatusChar = await service.getCharacteristic(uuids.status); + try { + const v = await entry.motionStatusChar.readValue(); + const msg = decodeJson(v); + if (msg?.st) entry.motionStatus = msg.st; + } catch {} + await entry.motionStatusChar.startNotifications(); + entry.motionStatusChar.addEventListener("characteristicvaluechanged", (e) => { + const msg = decodeJson(e.target.value); + if (!msg?.st) return; + entry.motionStatus = msg.st; + const sec = entry.node?.querySelector(`.cap-section[data-cap-name="${name}"]`); + if (sec) { + const stateEl = sec.querySelector(".cap-state"); + if (stateEl) stateEl.textContent = entry.motionStatus; + const bodyEl = sec.querySelector("[data-motion-status]"); + if (bodyEl) bodyEl.textContent = entry.motionStatus; + } else { + renderEntry(entry); + } + logFor(entry, `motion β†’ ${entry.motionStatus}`); + }); + startCvWatch(entry); + } catch (err) { + logFor(entry, `motion probe failed: ${err.message}`); + entry.motionGoalChar = null; + } + }, + + cleanup(entry) { + stopCvWatch(entry); + entry._motionJoypad?.destroy(); + entry._motionJoypad = null; + entry.motionGoalChar = entry.motionPoseChar = entry.motionStatusChar = null; + }, + + renderSection(entry) { + if (entry.status !== "connected" || !entry.motionGoalChar) return ""; + const uiMode = lsGet(entry, "mode", "manual"); + const ctrl = lsGet(entry, "ctrl", "spin_move_spin"); + const x = lsGet(entry, "x", "0"); + const y = lsGet(entry, "y", "0"); + const theta = lsGet(entry, "theta", "0"); + const wSep = lsGet(entry, "wheel_sep", ""); + const wR = lsGet(entry, "wheel_r", ""); + const maxSpd = lsGet(entry, "max_spd", ""); + const st = entry.motionStatus; + + const modeSeg = ` +
+ + +
`; + + const manualContent = ` +
+
+
`; + + const poseContent = ` +
+ + + +
+
+ + +
+
+ + + ${st} +
+
+ Wheel config +
+ + + +
+
`; + + const body = `${modeSeg}${uiMode === "manual" ? manualContent : poseContent}`; + + return capSection({ + name, + label: "Motion", + state: st, + action: ``, + body, + transport: "ble", + }); + }, + + wireActions(entry, node) { + entry._motionJoypad?.destroy(); + entry._motionJoypad = null; + + node.querySelector("[data-action='motion-tab-manual']")?.addEventListener("click", () => { + lsSet(entry, "mode", "manual"); + renderEntry(entry); + }); + node.querySelector("[data-action='motion-tab-pose']")?.addEventListener("click", () => { + lsSet(entry, "mode", "pose"); + renderEntry(entry); + }); + + node.querySelector("[data-action='motion-stop']")?.addEventListener("click", async () => { + entry._motionJoypad?.reset?.(); + if (entry.motionGoalChar) { + try { await entry.motionGoalChar.writeValueWithResponse(encodeJson({ op: "cancel" })); } + catch (err) { logFor(entry, `motion stop: ${err.message}`); } + } + setPairValue(entry, "motors", 0, 0); + }); + + const uiMode = lsGet(entry, "mode", "manual"); + + if (uiMode === "manual") { + const pad = node.querySelector(".joypad"); + const knob = pad?.querySelector(".joypad-knob"); + if (pad && knob) { + entry._motionJoypad = attachJoypad(pad, knob, { + onDrive: (l, r) => setPairValue(entry, "motors", l, r), + onStop: () => setPairValue(entry, "motors", 0, 0), + }); + } + } else { + const ctrlSms = node.querySelector("[data-action='motion-ctrl-sms']"); + const ctrlCont = node.querySelector("[data-action='motion-ctrl-cont']"); + ctrlSms?.addEventListener("click", () => { + lsSet(entry, "ctrl", "spin_move_spin"); + ctrlSms.setAttribute("aria-pressed", "true"); + ctrlCont?.setAttribute("aria-pressed", "false"); + }); + ctrlCont?.addEventListener("click", () => { + lsSet(entry, "ctrl", "continuous"); + ctrlCont.setAttribute("aria-pressed", "true"); + ctrlSms?.setAttribute("aria-pressed", "false"); + }); + + node.querySelectorAll("[data-motion-field]").forEach(el => { + el.addEventListener("change", () => lsSet(entry, el.dataset.motionField, el.value)); + }); + + node.querySelector("[data-action='motion-go']")?.addEventListener("click", async () => { + if (!entry.motionGoalChar) return; + const get = (a) => node.querySelector(`[data-action="${a}"]`)?.value; + const xVal = parseFloat(get("motion-x") || "0"); + const yVal = parseFloat(get("motion-y") || "0"); + const tDeg = parseFloat(get("motion-theta") || "0"); + const tRad = tDeg * (Math.PI / 180); + const ctrl = lsGet(entry, "ctrl", "spin_move_spin"); + const msg = { x: xVal, y: yVal, theta: tRad, mode: ctrl }; + const sep = parseFloat(get("motion-wheel-sep") || ""); + const rad = parseFloat(get("motion-wheel-r") || ""); + const spd = parseFloat(get("motion-max-spd") || ""); + if (sep > 0) msg.wheel_sep = sep; + if (rad > 0) msg.wheel_r = rad; + if (spd > 0) msg.max_spd = spd; + try { + await entry.motionGoalChar.writeValueWithResponse(encodeJson(msg)); + logFor(entry, `motion go (${xVal}, ${yVal}, ${tDeg}Β°) ${ctrl}`); + } catch (err) { + logFor(entry, `motion go failed: ${err.message}`); + } + }); + + node.querySelector("[data-action='motion-cancel']")?.addEventListener("click", async () => { + if (!entry.motionGoalChar) return; + try { await entry.motionGoalChar.writeValueWithResponse(encodeJson({ op: "cancel" })); } + catch (err) { logFor(entry, `motion cancel: ${err.message}`); } + }); + } + }, + }; +} diff --git a/public/capabilities/runtime/signed-pair.js b/public/capabilities/runtime/signed-pair.js index 839d8b7e..0e208245 100644 --- a/public/capabilities/runtime/signed-pair.js +++ b/public/capabilities/runtime/signed-pair.js @@ -220,8 +220,7 @@ function keyboardTick() { else if (axis === "turn+") turn = 100; else if (axis === "turn-") turn = -100; } - const [l, r] = mix(throttle, turn); - setPairValue(entry, "motors", l, r); + setPairValue(entry, "motors", throttle, turn); } function stopKeyboardMotors() { diff --git a/public/gamepad.js b/public/gamepad.js index eb691a1a..fbdb8d9b 100644 --- a/public/gamepad.js +++ b/public/gamepad.js @@ -39,23 +39,20 @@ function gamepadTick() { } const id = pickGamepadTarget(); if (id) { - const ly = pad.axes[1] ?? 0; // left stick Y (down = +1) - const ry = pad.axes[3] ?? 0; // right stick Y - const toMotor = (v) => { - const dz = Math.abs(v) < GAMEPAD_DEADZONE ? 0 : v; - return Math.round(-dz * 100); // invert so stick-up = forward - }; - const l = toMotor(ly); - const r = toMotor(ry); + const ly = pad.axes[1] ?? 0; // left stick Y (down = +1): forward + const rx = pad.axes[2] ?? 0; // right stick X (right = +1): turn + const dz = (v) => Math.abs(v) < GAMEPAD_DEADZONE ? 0 : v; + const forward = Math.round(-dz(ly) * 100); // up = +forward + const turn = Math.round( dz(rx) * 100); // right = +turn // Pi has a 500 ms motor watchdog β€” held-stick must keep refreshing it, // so only skip when both current and last frame are at-rest (0,0). // The transition to (0,0) still writes once so motors stop immediately // rather than waiting for the watchdog. - const atRest = l === 0 && r === 0 && _lastSent.l === 0 && _lastSent.r === 0 - && id === _lastSent.id; + const atRest = forward === 0 && turn === 0 + && _lastSent.l === 0 && _lastSent.r === 0 && id === _lastSent.id; if (!atRest) { - sendPairById(id, "motors", l, r); - _lastSent = { id, l, r }; + sendPairById(id, "motors", forward, turn); + _lastSent = { id, l: forward, r: turn }; } } renderGamepadBadge(id, pad.id); diff --git a/public/joypad.js b/public/joypad.js index 3db25e09..b1a38a20 100644 --- a/public/joypad.js +++ b/public/joypad.js @@ -46,7 +46,8 @@ export function attachJoypad(pad, knob, { onDrive, onStop, heartbeatMs = 200 } = const nx = Math.cos(angle) * dist; const ny = Math.sin(angle) * dist; knob.style.transform = `translate(${nx * radius}px, ${ny * radius}px)`; - [lastL, lastR] = mix(-ny * 100, nx * 100); // Y inverted: up = +throttle + lastL = -ny * 100; // forward%: up = +throttle + lastR = nx * 100; // turn%: right = +turn onDrive?.(lastL, lastR); }; diff --git a/public/styles.css b/public/styles.css index b91da40c..06d41c2a 100644 --- a/public/styles.css +++ b/public/styles.css @@ -1327,6 +1327,82 @@ details.tray.has-alert > summary::after { } .motor-sliders input[type="range"] { flex: 1; accent-color: var(--accent); } +/* Motion cap β€” pose control card with mode tabs, pose fields, and wheel config. */ +.motion-mode-seg { margin: 12px 0 8px; } +.motion-ctrl-seg { margin: 10px 0 6px; } +.motion-pose-fields { + display: flex; + gap: 8px; + margin-top: 10px; +} +.motion-pose-fields label { + flex: 1; + display: flex; + flex-direction: column; + gap: 4px; + font-size: 12px; + color: var(--ink-muted); +} +.motion-pose-fields input { + width: 100%; + min-width: 0; + padding: 4px 6px; + font-size: 14px; + border: 1px solid var(--line); + border-radius: 6px; + background: var(--bg-elev); + color: var(--ink); +} +.motion-go-row { + display: flex; + align-items: center; + gap: 8px; + margin-top: 10px; +} +.motion-status-pill { + margin-left: auto; + font-size: 12px; + color: var(--ink-muted); + padding: 2px 8px; + border-radius: 10px; + background: var(--bg-elev); + border: 1px solid var(--line); +} +.motion-wheel-config { + margin-top: 10px; + font-size: 13px; +} +.motion-wheel-config summary { + cursor: pointer; + color: var(--ink-muted); + padding: 4px 0; +} +.motion-wheel-fields { + display: flex; + flex-direction: column; + gap: 6px; + padding: 8px 0 4px; +} +.motion-wheel-fields label { + display: flex; + align-items: center; + gap: 8px; + font-size: 12px; + color: var(--ink-muted); +} +.motion-wheel-fields input { + flex: 1; + min-width: 0; + padding: 4px 6px; + font-size: 13px; + border: 1px solid var(--line); + border-radius: 6px; + background: var(--bg-elev); + color: var(--ink); +} +.motion-cv-fix { margin-top: 8px; } +.motion-cv-stale { opacity: 0.5; } + /* Level slider β€” single-axis brightness control (flash LED today). Sits in the cap-section action slot, where toggle/signed-pair would put a button. Width keeps it compact in the right column. */ diff --git a/public/sw.js b/public/sw.js index 096f2c26..a96a30fa 100644 --- a/public/sw.js +++ b/public/sw.js @@ -23,7 +23,7 @@ // commit. For an intentional bump unrelated to assets (e.g. server-side // change in an API contract), edit any cached asset (a comment will do) // and the hook will pick up a new hash. -const VERSION = "bac00880"; +const VERSION = "e04a4bee"; const CACHE = `dashboard-${VERSION}`; // Cached at install time so the dashboard can cold-boot offline AND diff --git a/public/uuids.js b/public/uuids.js index c3e90c1c..c792ecd2 100644 --- a/public/uuids.js +++ b/public/uuids.js @@ -1,4 +1,4 @@ -// Generated by tools/gen-uuids.py from protocol/uuids.json β€” do not edit. +// Generated by tools/gen-uuids.py from protocol/uuids.json — do not edit. // Run `make gen-uuids` (or `python3 tools/gen-uuids.py`) after editing the JSON. // main_service @@ -22,6 +22,9 @@ export const SNAPSHOT_DATA_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da1"; export const FLASH_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da3"; export const PIN_CONFIG_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da4"; export const SIGNAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da5"; +export const MOTION_GOAL_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da6"; +export const MOTION_POSE_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da7"; +export const MOTION_STATUS_CHAR_UUID = "a5f7c4d2-1b8e-4b9a-9c3d-5e8a7b6c4da8"; // heartbeat_service export const HEARTBEAT_SVC_UUID = "b6e8d5f3-2c9d-4bba-ae5e-6f9b8c7d5eb0";