From 96cafa672d2819fd5cd5810c90f8541ba4b9ec76 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 19:04:36 -0400 Subject: [PATCH 1/6] Motion: add diff drive pose controller + BLE integration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds packages/control β€” a C++ library with two PoseController implementations (continuous and spin-move-spin), diff drive kinematics, and pybind11 bindings for future compiled use. Adds firmware/pi_robot/motion_control.py β€” pure Python mirror of the C++ interface that runs on the Pi today. Wires into pi_robot.py: three new BLE characteristics (motion-goal, motion-pose, motion-status), wheel config from pi-robot.conf, 20 Hz tick loop, and automatic cancellation on manual joystick input. Co-Authored-By: Claude Sonnet 4.6 --- firmware/esp32_robot_idf/main/uuids.h | 5 +- firmware/pi_robot/motion_control.py | 186 ++++++++++++++++++ firmware/pi_robot/pi_robot.py | 162 ++++++++++++++- firmware/pi_robot/uuids.py | 5 +- packages/control/CMakeLists.txt | 27 +++ .../include/control/continuous_controller.hpp | 30 +++ .../include/control/control_output.hpp | 6 + .../control/include/control/diff_drive.hpp | 15 ++ packages/control/include/control/pose2d.hpp | 14 ++ .../include/control/pose_controller.hpp | 12 ++ .../control/spin_move_spin_controller.hpp | 30 +++ packages/control/python/bindings.cpp | 54 +++++ .../control/src/continuous_controller.cpp | 50 +++++ packages/control/src/diff_drive.cpp | 14 ++ .../control/src/spin_move_spin_controller.cpp | 66 +++++++ protocol/uuids.json | 5 +- public/uuids.js | 5 +- 17 files changed, 680 insertions(+), 6 deletions(-) create mode 100644 firmware/pi_robot/motion_control.py create mode 100644 packages/control/CMakeLists.txt create mode 100644 packages/control/include/control/continuous_controller.hpp create mode 100644 packages/control/include/control/control_output.hpp create mode 100644 packages/control/include/control/diff_drive.hpp create mode 100644 packages/control/include/control/pose2d.hpp create mode 100644 packages/control/include/control/pose_controller.hpp create mode 100644 packages/control/include/control/spin_move_spin_controller.hpp create mode 100644 packages/control/python/bindings.cpp create mode 100644 packages/control/src/continuous_controller.cpp create mode 100644 packages/control/src/diff_drive.cpp create mode 100644 packages/control/src/spin_move_spin_controller.cpp 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 fc503d1b..91538ee6 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: @@ -158,7 +163,10 @@ def _load_config() -> dict: "left": {"forward": 5, "backward": 6}, "right": {"forward": 13, "backward": 26}, }) -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] @@ -241,6 +249,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") @@ -307,6 +325,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 @@ -810,6 +833,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. @@ -1309,6 +1334,105 @@ 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) -> 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 + cfg = DiffDriveConfig(WHEEL_SEPARATION, WHEEL_RADIUS, MAX_WHEEL_SPEED) + 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", x, y, theta, mode) + + +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")) + _schedule(_motion_start(x, y, theta, mode)) + + +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: @@ -1389,6 +1513,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 @@ -1431,6 +1557,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: @@ -1579,6 +1711,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.") @@ -1586,7 +1742,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/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"; From 08994a27102a113d0f99cfd4a96d24d3f6459295 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 19:14:26 -0400 Subject: [PATCH 2/6] Motors: proper diff drive kinematics for manual joystick control MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Redefines the 2-byte motor protocol as (forward%, turn%) instead of (left%, right%). The Pi converts to physical (v, Ο‰) using wheel_separation and max_wheel_speed from pi-robot.conf, then applies diff drive kinematics to get correct wheel PWM values. Handles backward turns correctly without a sign flip. Dashboard senders updated to match: - joypad: sends raw (forward, turn) instead of pre-mixed (left, right) - keyboard: removes mix() call, sends (throttle, turn) directly - gamepad: switches from dual-stick tank to left-Y forward + right-X turn The 4-byte LLM pulse path is unchanged. Co-Authored-By: Claude Sonnet 4.6 --- firmware/pi_robot/pi_robot.py | 15 ++++++++++++++- public/capabilities/runtime/signed-pair.js | 3 +-- public/gamepad.js | 21 +++++++++------------ public/joypad.js | 3 ++- 4 files changed, 26 insertions(+), 16 deletions(-) diff --git a/firmware/pi_robot/pi_robot.py b/firmware/pi_robot/pi_robot.py index 91538ee6..61933e66 100644 --- a/firmware/pi_robot/pi_robot.py +++ b/firmware/pi_robot/pi_robot.py @@ -851,7 +851,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]))) 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); }; From 9dc4f410a362c59225befdd21e7028aaaac3698c Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 19:35:56 -0400 Subject: [PATCH 3/6] Dashboard: motion control card with manual joypad + pose goal UI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds a Motion cap card (pose-control type) that appears on Pi robots with motors enabled. Supports two modes selectable per robot: - Manual: virtual joypad wired to motorsChar (same diff-drive kinematics as the Motors card) - Pose Control: X/Y (m) + theta (Β°) inputs, controller toggle (Spinβ†’Moveβ†’Spin / Continuous), Go/Cancel buttons, live status pill, collapsible wheel-config overrides (separation, radius, max speed) Dashboard changes: - public/capabilities/runtime/motion.js β€” new makeMotionCap - public/ble.js β€” UUIDS_BY_CAP.motion + import 3 motion UUIDs - public/capabilities/runtime/index.js β€” register "pose-control" type - public/capabilities/runtime/cap-section.js β€” motion defaults open - public/styles.css β€” motion card layout styles Firmware changes: - pi_robot.py _motion_start + _motion_goal_handle_write now accept optional wheel_sep / wheel_r / max_spd overrides from the goal message; fall back to pi-robot.conf values when absent Co-Authored-By: Claude Sonnet 4.6 --- firmware/pi_robot/pi_robot.py | 19 +- public/ble.js | 2 + public/capabilities/runtime/cap-section.js | 2 +- public/capabilities/runtime/index.js | 2 + public/capabilities/runtime/motion.js | 242 +++++++++++++++++++++ public/styles.css | 74 +++++++ 6 files changed, 335 insertions(+), 6 deletions(-) create mode 100644 public/capabilities/runtime/motion.js diff --git a/firmware/pi_robot/pi_robot.py b/firmware/pi_robot/pi_robot.py index 61933e66..65aa5a8d 100644 --- a/firmware/pi_robot/pi_robot.py +++ b/firmware/pi_robot/pi_robot.py @@ -1393,7 +1393,9 @@ async def _motion_tick_loop(cfg) -> None: raise -async def _motion_start(x: float, y: float, theta: float, mode: str) -> None: +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() @@ -1401,14 +1403,18 @@ async def _motion_start(x: float, y: float, theta: float, mode: str) -> None: if _motion_task is not None and not _motion_task.done(): _motion_task.cancel() _motion_task = None - cfg = DiffDriveConfig(WHEEL_SEPARATION, WHEEL_RADIUS, MAX_WHEEL_SPEED) + 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", x, y, theta, mode) + 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: @@ -1429,8 +1435,11 @@ def _motion_goal_handle_write(data: bytearray) -> None: except (KeyError, ValueError) as e: log.warning("motion-goal: missing field β€” %s", e) return - mode = str(msg.get("mode", "spin_move_spin")) - _schedule(_motion_start(x, y, theta, mode)) + 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: 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 d1d45f31..14382694 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..ca021623 --- /dev/null +++ b/public/capabilities/runtime/motion.js @@ -0,0 +1,242 @@ +// 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. + +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"; + +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 {} +} + +export function makeMotionCap(schema) { + const { name } = schema; + + return { + name, + schema, + + initEntry: () => ({ + motionGoalChar: null, + motionPoseChar: null, + motionStatusChar: null, + motionStatus: "idle", + _motionJoypad: 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; + // Surgical update β€” avoid full re-render during active movement. + 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}`); + }); + } catch (err) { + logFor(entry, `motion probe failed: ${err.message}`); + entry.motionGoalChar = null; + } + }, + + cleanup(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) { + // Destroy any in-progress joypad drag from the previous render cycle. + entry._motionJoypad?.destroy(); + entry._motionJoypad = null; + + // Mode tabs + 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); + }); + + // Stop β€” cancel goal + zero motors + 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 { + // Controller type toggle β€” surgical aria-pressed swap, no re-render. + 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"); + }); + + // Persist field edits to localStorage on blur/enter. + node.querySelectorAll("[data-motion-field]").forEach(el => { + el.addEventListener("change", () => lsSet(entry, el.dataset.motionField, el.value)); + }); + + // Go + 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}`); + } + }); + + // Cancel + 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/styles.css b/public/styles.css index 299644e8..6e17c1b8 100644 --- a/public/styles.css +++ b/public/styles.css @@ -1256,6 +1256,80 @@ 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); +} + /* 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. */ From 1392554b44e075603df07a06029c6de6a668a9d5 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 19:37:03 -0400 Subject: [PATCH 4/6] =?UTF-8?q?Motion:=20rename=20controller=20toggle=20la?= =?UTF-8?q?bel=20to=20Pose=20=C2=B7=20Move=20=C2=B7=20Pose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Sonnet 4.6 --- public/capabilities/runtime/motion.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/public/capabilities/runtime/motion.js b/public/capabilities/runtime/motion.js index ca021623..fa164e67 100644 --- a/public/capabilities/runtime/motion.js +++ b/public/capabilities/runtime/motion.js @@ -114,7 +114,7 @@ export function makeMotionCap(schema) {
+ aria-pressed="${ctrl === "spin_move_spin"}" type="button">Pose Β· Move Β· Pose
From 151bbf6f67dbd02b2a5da3e6023fe31171232e14 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 20:29:17 -0400 Subject: [PATCH 5/6] Motion: auto-apply CV ground-truth fix before each Go Before sending a pose goal, write entry.cvPosition to motionPoseChar so the robot's odometry starts from the camera-measured position rather than its dead-reckoned estimate. Fix is skipped if cvPosition is absent or stale (> 10s old). The pose tab shows the last CV fix and dims it when stale. No-ops until the cv branch is merged and entry.cvPosition is populated. Co-Authored-By: Claude Sonnet 4.6 --- public/capabilities/runtime/motion.js | 30 +++++++++++++++++++++++++++ public/styles.css | 2 ++ 2 files changed, 32 insertions(+) diff --git a/public/capabilities/runtime/motion.js b/public/capabilities/runtime/motion.js index fa164e67..262d39ca 100644 --- a/public/capabilities/runtime/motion.js +++ b/public/capabilities/runtime/motion.js @@ -12,6 +12,16 @@ import { capSection } from "./cap-section.js"; import { renderEntry } from "./render-bus.js"; const LS_PREFIX = "better-robotics:motion"; +const CV_STALE_MS = 10_000; // don't apply a fix older than this + +function cvFixLine(entry) { + const p = entry.cvPosition; + if (!p) return `
CV fix: none β€” overhead camera not running
`; + const age = Math.round((Date.now() - p.updatedAt) / 1000); + const stale = (Date.now() - p.updatedAt) > CV_STALE_MS; + const detail = `(${p.x}, ${p.y} mm Β· ${p.headingDeg}Β°) Β· ${age}s ago`; + return `
CV fix: ${stale ? "stale β€” " : ""}${detail}
`; +} function lsGet(entry, k, def = "") { try { @@ -123,6 +133,7 @@ export function makeMotionCap(schema) { ${st} + ${cvFixLine(entry)}
Wheel config
@@ -222,6 +233,25 @@ export function makeMotionCap(schema) { if (sep > 0) msg.wheel_sep = sep; if (rad > 0) msg.wheel_r = rad; if (spd > 0) msg.max_spd = spd; + + // Apply CV ground-truth fix before sending the goal so the robot's + // odometry starts from the camera-measured position rather than its + // dead-reckoned estimate. Skip if no fix or fix is stale (> 10s). + const p = entry.cvPosition; + if (p && entry.motionPoseChar && (Date.now() - p.updatedAt) < CV_STALE_MS) { + try { + const fix = { + x: p.x / 1000, // mm β†’ m + y: p.y / 1000, + theta: p.headingDeg * (Math.PI / 180), // deg β†’ rad + }; + await entry.motionPoseChar.writeValueWithResponse(encodeJson(fix)); + logFor(entry, `motion cv fix: (${p.x}, ${p.y} mm, ${p.headingDeg}Β°)`); + } catch (err) { + logFor(entry, `motion cv fix failed: ${err.message}`); + } + } + try { await entry.motionGoalChar.writeValueWithResponse(encodeJson(msg)); logFor(entry, `motion go (${xVal}, ${yVal}, ${tDeg}Β°) ${ctrl}`); diff --git a/public/styles.css b/public/styles.css index 6e17c1b8..50b538d8 100644 --- a/public/styles.css +++ b/public/styles.css @@ -1329,6 +1329,8 @@ details.tray.has-alert > summary::after { 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 From 23dbd698dec4ba79b9bcd7ba5f03b479391d5ed3 Mon Sep 17 00:00:00 2001 From: JaredBaileyDuke Date: Mon, 11 May 2026 20:33:56 -0400 Subject: [PATCH 6/6] Motion: background CV pose correction via polling interval Replace the Go-button-triggered CV fix with a 500ms polling loop (startCvWatch/stopCvWatch) that automatically writes odometry resets to motionPoseChar whenever a fresh, non-stale cvPosition arrives. No user interaction required; no CV state visible in the motion card. Co-Authored-By: Claude Sonnet 4.6 --- public/capabilities/runtime/motion.js | 89 ++++++++++++++------------- 1 file changed, 46 insertions(+), 43 deletions(-) diff --git a/public/capabilities/runtime/motion.js b/public/capabilities/runtime/motion.js index 262d39ca..84d4363c 100644 --- a/public/capabilities/runtime/motion.js +++ b/public/capabilities/runtime/motion.js @@ -3,6 +3,10 @@ // 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"; @@ -12,16 +16,8 @@ import { capSection } from "./cap-section.js"; import { renderEntry } from "./render-bus.js"; const LS_PREFIX = "better-robotics:motion"; -const CV_STALE_MS = 10_000; // don't apply a fix older than this - -function cvFixLine(entry) { - const p = entry.cvPosition; - if (!p) return `
CV fix: none β€” overhead camera not running
`; - const age = Math.round((Date.now() - p.updatedAt) / 1000); - const stale = (Date.now() - p.updatedAt) > CV_STALE_MS; - const detail = `(${p.x}, ${p.y} mm Β· ${p.headingDeg}Β°) Β· ${age}s ago`; - return `
CV fix: ${stale ? "stale β€” " : ""}${detail}
`; -} +const CV_STALE_MS = 10_000; +const CV_POLL_MS = 500; function lsGet(entry, k, def = "") { try { @@ -33,6 +29,38 @@ 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; @@ -41,11 +69,12 @@ export function makeMotionCap(schema) { schema, initEntry: () => ({ - motionGoalChar: null, - motionPoseChar: null, - motionStatusChar: null, - motionStatus: "idle", - _motionJoypad: null, + motionGoalChar: null, + motionPoseChar: null, + motionStatusChar: null, + motionStatus: "idle", + _motionJoypad: null, + _cvWatchInterval: null, }), async probe(entry, service) { @@ -64,7 +93,6 @@ export function makeMotionCap(schema) { const msg = decodeJson(e.target.value); if (!msg?.st) return; entry.motionStatus = msg.st; - // Surgical update β€” avoid full re-render during active movement. const sec = entry.node?.querySelector(`.cap-section[data-cap-name="${name}"]`); if (sec) { const stateEl = sec.querySelector(".cap-state"); @@ -76,6 +104,7 @@ export function makeMotionCap(schema) { } logFor(entry, `motion β†’ ${entry.motionStatus}`); }); + startCvWatch(entry); } catch (err) { logFor(entry, `motion probe failed: ${err.message}`); entry.motionGoalChar = null; @@ -83,6 +112,7 @@ export function makeMotionCap(schema) { }, cleanup(entry) { + stopCvWatch(entry); entry._motionJoypad?.destroy(); entry._motionJoypad = null; entry.motionGoalChar = entry.motionPoseChar = entry.motionStatusChar = null; @@ -133,7 +163,6 @@ export function makeMotionCap(schema) { ${st}
- ${cvFixLine(entry)}
Wheel config
@@ -162,11 +191,9 @@ export function makeMotionCap(schema) { }, wireActions(entry, node) { - // Destroy any in-progress joypad drag from the previous render cycle. entry._motionJoypad?.destroy(); entry._motionJoypad = null; - // Mode tabs node.querySelector("[data-action='motion-tab-manual']")?.addEventListener("click", () => { lsSet(entry, "mode", "manual"); renderEntry(entry); @@ -176,7 +203,6 @@ export function makeMotionCap(schema) { renderEntry(entry); }); - // Stop β€” cancel goal + zero motors node.querySelector("[data-action='motion-stop']")?.addEventListener("click", async () => { entry._motionJoypad?.reset?.(); if (entry.motionGoalChar) { @@ -198,7 +224,6 @@ export function makeMotionCap(schema) { }); } } else { - // Controller type toggle β€” surgical aria-pressed swap, no re-render. const ctrlSms = node.querySelector("[data-action='motion-ctrl-sms']"); const ctrlCont = node.querySelector("[data-action='motion-ctrl-cont']"); ctrlSms?.addEventListener("click", () => { @@ -212,12 +237,10 @@ export function makeMotionCap(schema) { ctrlSms?.setAttribute("aria-pressed", "false"); }); - // Persist field edits to localStorage on blur/enter. node.querySelectorAll("[data-motion-field]").forEach(el => { el.addEventListener("change", () => lsSet(entry, el.dataset.motionField, el.value)); }); - // Go node.querySelector("[data-action='motion-go']")?.addEventListener("click", async () => { if (!entry.motionGoalChar) return; const get = (a) => node.querySelector(`[data-action="${a}"]`)?.value; @@ -233,25 +256,6 @@ export function makeMotionCap(schema) { if (sep > 0) msg.wheel_sep = sep; if (rad > 0) msg.wheel_r = rad; if (spd > 0) msg.max_spd = spd; - - // Apply CV ground-truth fix before sending the goal so the robot's - // odometry starts from the camera-measured position rather than its - // dead-reckoned estimate. Skip if no fix or fix is stale (> 10s). - const p = entry.cvPosition; - if (p && entry.motionPoseChar && (Date.now() - p.updatedAt) < CV_STALE_MS) { - try { - const fix = { - x: p.x / 1000, // mm β†’ m - y: p.y / 1000, - theta: p.headingDeg * (Math.PI / 180), // deg β†’ rad - }; - await entry.motionPoseChar.writeValueWithResponse(encodeJson(fix)); - logFor(entry, `motion cv fix: (${p.x}, ${p.y} mm, ${p.headingDeg}Β°)`); - } catch (err) { - logFor(entry, `motion cv fix failed: ${err.message}`); - } - } - try { await entry.motionGoalChar.writeValueWithResponse(encodeJson(msg)); logFor(entry, `motion go (${xVal}, ${yVal}, ${tDeg}Β°) ${ctrl}`); @@ -260,7 +264,6 @@ export function makeMotionCap(schema) { } }); - // Cancel node.querySelector("[data-action='motion-cancel']")?.addEventListener("click", async () => { if (!entry.motionGoalChar) return; try { await entry.motionGoalChar.writeValueWithResponse(encodeJson({ op: "cancel" })); }