diff --git a/src/components/dashboard/AddWidget.tsx b/src/components/dashboard/AddWidget.tsx index cf6ccf1..264abd7 100644 --- a/src/components/dashboard/AddWidget.tsx +++ b/src/components/dashboard/AddWidget.tsx @@ -109,6 +109,27 @@ const AddWidgets: React.FC = () => { > {'Custom Variable'} + + {/* Robot Control (puppet mode: drive motors, servos and LED from the dashboard) */} + { + addWidget(() => ({ + h: 9, + w: 4, + x: 0, + y: 2, + minW: 2, + minH: 5, + content: JSON.stringify({ + name: 'PuppetControl', + props: {}, + }), + })); + }} + > + {'Robot Control'} + {customSensors.length > 0 && ( <>
diff --git a/src/components/dashboard/sensors/PuppetControlWidget.tsx b/src/components/dashboard/sensors/PuppetControlWidget.tsx new file mode 100644 index 0000000..460d8ba --- /dev/null +++ b/src/components/dashboard/sensors/PuppetControlWidget.tsx @@ -0,0 +1,558 @@ +import React, { useState, useEffect, useCallback, useRef } from 'react'; +import { FaTrash, FaPlay, FaStop, FaSlidersH, FaLightbulb, FaHandPaper } from 'react-icons/fa'; +import AppMgr, { EventType } from '@/managers/appmgr'; +import { CustomVarMeta, NetworkTable } from '@/managers/tablemgr'; +import { useGridStackWidget } from '../hooks/useGridStackWidget'; +import { sendVariableUpdate } from '../utils/xppUtils'; +import { startPuppetPassthrough, stopPuppetPassthrough } from '../utils/puppetMode'; +import SensorCard from './SensorCard'; + +/** Variables the puppet passthrough defines on the robot. */ +const SERVO_VAR_PATTERN = /^\$servo\.([1-4])$/; +const MOTOR_VAR_PATTERN = /^\$motor\.(left|right|3|4)$/; +const LED_VAR = '$led'; +const KEEPALIVE_VAR = '$puppet.keepalive'; +const BUTTON_VAR = '$button'; +const BOARD_TYPE_VAR = '$board.type'; +const STRAIGHT_VAR = '$drivetrain.straight'; +const STRAIGHT_EFFORT_VAR = '$drivetrain.straight.effort'; +const TURN_VAR = '$drivetrain.turn'; +const TURN_EFFORT_VAR = '$drivetrain.turn.effort'; +const DRIVE_BUSY_VAR = '$drivetrain.busy'; + +/** Board type codes reported by the passthrough (XPP has no string type). */ +const BOARD_TYPE_NAMES: Record = { + 0: 'Beta XRP', + 1: 'XRP', + 2: 'NanoXRP', +}; + +/** Display order and labels for the motor channels. */ +const MOTOR_ORDER: [string, string][] = [ + ['$motor.left', 'Left'], + ['$motor.right', 'Right'], + ['$motor.3', 'Motor 3'], + ['$motor.4', 'Motor 4'], +]; + +/** A drive magnitude (cm or degrees) is valid when finite and nonzero. */ +const isValidDistance = (s: string): boolean => { + const v = Number(s); + return isFinite(v) && v !== 0; +}; + +/** A drive max effort is valid in (0, 1] — 0 would sit still until timeout. */ +const isValidEffort = (s: string): boolean => { + const v = Number(s); + return isFinite(v) && v > 0 && v <= 1; +}; + +/** Pacing floor between XPP update rounds while a slider is dragged. */ +const SEND_INTERVAL_MS = 30; + +/** Keepalive period; the robot stops its motors if the link goes quiet. */ +const KEEPALIVE_INTERVAL_MS = 500; + +/** + * Robot control widget for puppet mode. + * + * "Start puppet" runs /XRPExamples/puppet_passthrough.py on the robot via the + * same machinery as the editor's Run button. The robot answers by defining + * its puppet variables (only the channels the board actually has), which + * enables the controls: servo angle sliders, motor effort sliders and the + * LED toggle. Control moves stream XPP variable updates back to the robot, + * and a periodic keepalive feeds the robot-side motor watchdog. + */ +const PuppetControlWidget: React.FC = () => { + const { handleDelete } = useGridStackWidget(); + + // Puppet variable name → metadata, discovered from NetworkTable VAR_DEFs + const [varMeta, setVarMeta] = useState>({}); + // Slider/toggle positions (angles in degrees, efforts in percent) + const [angles, setAngles] = useState>({}); + const [efforts, setEfforts] = useState>({}); + const [ledOn, setLedOn] = useState(false); + // User button state pushed by the robot (null until the first update) + const [buttonPressed, setButtonPressed] = useState(null); + // Board type name reported by the robot (null until the first update) + const [boardType, setBoardType] = useState(null); + // Drivetrain state: straight() blocks the robot's loop, so $drivetrain.busy + // is the only feedback while a drive runs + const [driveBusy, setDriveBusy] = useState(false); + // Blocking-command parameters: distance/degrees plus max effort (0-1) + const [driveDistance, setDriveDistance] = useState('30'); + const [straightEffort, setStraightEffort] = useState('0.5'); + const [turnDegrees, setTurnDegrees] = useState('90'); + const [turnEffort, setTurnEffort] = useState('0.5'); + const [isStarting, setIsStarting] = useState(false); + const [startError, setStartError] = useState(false); + + // Self-pacing send pump. While a write is in flight, newer slider values + // just overwrite the pending slot, so the next send always carries the + // newest setpoint and intermediate positions are dropped instead of queued. + // (A fixed-rate sender backlogs on slow links — BLE GATT writes especially — + // and the actuators then crawl through stale positions long after the drag.) + const pendingValues = useRef>({}); + const pumpRunning = useRef(false); + const metaRef = useRef(varMeta); + metaRef.current = varMeta; + // Set once the robot itself reports a drive in progress; used to clear an + // optimistic busy state when the Go command never made it to the robot + const driveConfirmed = useRef(false); + + useEffect(() => { + const handleData = (data: string) => { + try { + const table: NetworkTable = JSON.parse(data); + const meta = table.__customVarMeta as Record | undefined; + if (!meta) return; + + const puppetVars: Record = {}; + for (const [name, entry] of Object.entries(meta)) { + if ( + SERVO_VAR_PATTERN.test(name) || + MOTOR_VAR_PATTERN.test(name) || + name === LED_VAR || + name === KEEPALIVE_VAR || + name === BUTTON_VAR || + name === BOARD_TYPE_VAR || + name === STRAIGHT_VAR || + name === STRAIGHT_EFFORT_VAR || + name === TURN_VAR || + name === TURN_EFFORT_VAR || + name === DRIVE_BUSY_VAR + ) { + puppetVars[name] = entry; + } + } + + // Drive-in-progress flag, pushed by the robot around straight() + const busyValue = table[DRIVE_BUSY_VAR]; + if (typeof busyValue === 'number') { + if (busyValue !== 0) driveConfirmed.current = true; + setDriveBusy(busyValue !== 0); + } + + // The robot pushes the button state on change (edge interrupt); + // TableMgr exposes it as a top-level 1/0 value. + const buttonValue = table[BUTTON_VAR]; + if (typeof buttonValue === 'number') { + setButtonPressed(buttonValue !== 0); + } + + // Board type is sent once at passthrough startup. + const boardValue = table[BOARD_TYPE_VAR]; + if (typeof boardValue === 'number') { + setBoardType(BOARD_TYPE_NAMES[boardValue] ?? `Unknown (${boardValue})`); + } + + // Only touch state when the set of puppet definitions changes + setVarMeta((prev) => { + const prevKeys = Object.keys(prev).sort().join(); + const nextKeys = Object.keys(puppetVars).sort().join(); + return prevKeys === nextKeys ? prev : puppetVars; + }); + } catch { + // ignore parse errors + } + }; + + AppMgr.getInstance().on(EventType.EVENT_DASHBOARD_DATA, handleData); + return () => { + }; + }, []); + + // Keepalive ticker: feeds the robot-side watchdog that stops the motors + // when the browser goes away (tab closed, cable pulled, BLE drop). + useEffect(() => { + const meta = varMeta[KEEPALIVE_VAR]; + if (!meta) return; + + let tick = 0; + const id = setInterval(() => { + tick = (tick + 1) % 0x7fffffff; + void sendVariableUpdate(meta, tick); + }, KEEPALIVE_INTERVAL_MS); + return () => clearInterval(id); + }, [varMeta]); + + const queueSend = useCallback((name: string, value: number) => { + pendingValues.current[name] = value; + if (pumpRunning.current) return; // pump will pick this value up + + pumpRunning.current = true; + void (async () => { + try { + while (Object.keys(pendingValues.current).length > 0) { + // Take a snapshot; anything set while we await lands in the next round + const batch = pendingValues.current; + pendingValues.current = {}; + for (const [varName, value] of Object.entries(batch)) { + const meta = metaRef.current[varName]; + if (meta) { + await sendVariableUpdate(meta, value); + } + } + // Pacing floor so a fast link doesn't spam the robot + await new Promise((resolve) => setTimeout(resolve, SEND_INTERVAL_MS)); + } + } finally { + pumpRunning.current = false; + } + })(); + }, []); + + const handleAngleChange = useCallback((name: string, value: number) => { + setAngles((prev) => ({ ...prev, [name]: value })); + queueSend(name, value); + }, [queueSend]); + + const handleEffortChange = useCallback((name: string, percent: number) => { + setEfforts((prev) => ({ ...prev, [name]: percent })); + queueSend(name, percent / 100); + }, [queueSend]); + + const handleLedToggle = useCallback(() => { + setLedOn((prev) => { + queueSend(LED_VAR, prev ? 0 : 1); + return !prev; + }); + }, [queueSend]); + + const handleMotorsStop = useCallback(() => { + setEfforts((prev) => { + const zeroed: Record = {}; + for (const key of Object.keys(prev)) zeroed[key] = 0; + return zeroed; + }); + for (const [name] of MOTOR_ORDER) { + if (metaRef.current[name]) { + queueSend(name, 0); + } + } + }, [queueSend]); + + /** + * Start a blocking drivetrain command (straight or turn). The effort + * parameter is written first, then the command trigger — variable writes + * apply in order on the robot, so the effort is in place before the + * motion starts. The motion ends with the motors stopped and the robot + * discards any efforts that arrive mid-motion, so the motor sliders are + * zeroed to match. Sent directly (not via the pump): discrete commands + * must never be coalesced away like slider intermediates. + */ + const startBlockingDrive = useCallback(async ( + commandVar: string, valueStr: string, + effortVar: string, effortStr: string, + ) => { + const commandMeta = metaRef.current[commandVar]; + const effortMeta = metaRef.current[effortVar]; + const value = Number(valueStr); + const effort = Number(effortStr); + if (!commandMeta || !isFinite(value) || value === 0) return; + if (!isFinite(effort) || effort <= 0 || effort > 1) return; + + setEfforts((prev) => { + const zeroed: Record = {}; + for (const key of Object.keys(prev)) zeroed[key] = 0; + return zeroed; + }); + setDriveBusy(true); // optimistic; the robot's busy update confirms it + driveConfirmed.current = false; + + if (effortMeta) { + await sendVariableUpdate(effortMeta, effort); + } + void sendVariableUpdate(commandMeta, value); + + // If the command was lost (e.g. a BLE hiccup) the robot never reports + // busy — clear the optimistic state instead of leaving the controls + // disabled forever. + setTimeout(() => { + if (!driveConfirmed.current) { + setDriveBusy(false); + } + }, 3000); + }, []); + + const handleStart = useCallback(async () => { + setIsStarting(true); + setStartError(false); + const ok = await startPuppetPassthrough(); + setIsStarting(false); + setStartError(!ok); + }, []); + + const servoNames = Object.keys(varMeta).filter((n) => SERVO_VAR_PATTERN.test(n)).sort(); + const motorEntries = MOTOR_ORDER.filter(([name]) => varMeta[name]); + const hasLed = LED_VAR in varMeta; + const hasButton = BUTTON_VAR in varMeta; + const hasStraight = STRAIGHT_VAR in varMeta; + const hasTurn = TURN_VAR in varMeta; + const hasDrive = hasStraight || hasTurn; + const isLive = servoNames.length > 0 || motorEntries.length > 0 || hasLed || hasButton || hasDrive; + + return ( + } + onStart={() => { }} + onStop={() => { }} + isConnected={isLive} + > +
+ +
+ +
+ {/* Puppet mode start/stop */} +
+ + + {isLive && boardType && ( + + {boardType} + + )} +
+ + {!isLive ? ( +
+
+ Start puppet mode to control the robot +
+
+ Runs XRPExamples/puppet_passthrough.py on the robot + (requires the XRPLib examples to be installed) +
+ {startError && ( +
+ Could not start — check that the XRP is connected +
+ )} +
+ ) : ( +
+ {/* LED */} + {hasLed && ( +
+ LED + +
+ )} + + {/* User button (read-only; robot pushes changes via edge interrupt) */} + {hasButton && ( +
+ Button + + {buttonPressed === null ? '—' : buttonPressed ? 'Pressed' : 'Released'} + +
+ )} + + {/* Drivetrain: continuous effort plus the blocking straight/turn + commands (controls freeze until a blocking motion completes, + so everything is gated on busy) */} + {hasDrive && ( +
+
+ Drivetrain + {driveBusy && ( + + controls paused until the robot arrives + + )} +
+ {hasStraight && ( +
+ Straight + setDriveDistance(e.target.value)} + disabled={driveBusy} + step="any" + className="w-20 px-2 py-1 text-sm border border-gray-300 rounded font-mono dark:bg-gray-700 dark:border-gray-600 dark:text-white disabled:opacity-50" + title="Distance in cm (negative drives backward)" + /> + cm + setStraightEffort(e.target.value)} + disabled={driveBusy} + min={0} + max={1} + step={0.05} + className="w-16 px-2 py-1 text-sm border border-gray-300 rounded font-mono dark:bg-gray-700 dark:border-gray-600 dark:text-white disabled:opacity-50" + title="Max effort, 0-1" + /> + effort + +
+ )} + {hasTurn && ( +
+ Turn + setTurnDegrees(e.target.value)} + disabled={driveBusy} + step="any" + className="w-20 px-2 py-1 text-sm border border-gray-300 rounded font-mono dark:bg-gray-700 dark:border-gray-600 dark:text-white disabled:opacity-50" + title="Degrees to turn (positive = counterclockwise)" + /> + ° + setTurnEffort(e.target.value)} + disabled={driveBusy} + min={0} + max={1} + step={0.05} + className="w-16 px-2 py-1 text-sm border border-gray-300 rounded font-mono dark:bg-gray-700 dark:border-gray-600 dark:text-white disabled:opacity-50" + title="Max effort, 0-1" + /> + effort + +
+ )} +
+ )} + + {/* Motor efforts */} + {motorEntries.length > 0 && ( +
+
+ Motors + +
+ {motorEntries.map(([name, label]) => { + const percent = efforts[name] ?? 0; + return ( +
+ + {label} + + handleEffortChange(name, Number(e.target.value))} + className="flex-1 accent-blue-500 disabled:opacity-50" + /> + + {percent}% + +
+ ); + })} +
+ )} + + {/* Servo angles */} + {servoNames.length > 0 && ( +
+ Servos + {servoNames.map((name) => { + const index = name.match(SERVO_VAR_PATTERN)?.[1] ?? '?'; + const angle = angles[name] ?? 90; + return ( +
+ + Servo {index} + + handleAngleChange(name, Number(e.target.value))} + className="flex-1 accent-blue-500 disabled:opacity-50" + /> + + {angle}° + +
+ ); + })} +
+ )} +
+ )} +
+
+ ); +}; + +export default PuppetControlWidget; diff --git a/src/components/dashboard/utils/puppetMode.ts b/src/components/dashboard/utils/puppetMode.ts new file mode 100644 index 0000000..65b763a --- /dev/null +++ b/src/components/dashboard/utils/puppetMode.ts @@ -0,0 +1,48 @@ +import AppMgr from '@/managers/appmgr'; +import { CommandToXRPMgr } from '@/managers/commandstoxrpmgr'; + +/** + * Puppet mode: run the XRPLib puppet passthrough program on the robot so the + * dashboard can drive its actuators directly over XPP variable updates. + * + * The switch rides the same machinery as the editor's Run button + * (stopProgram → updateMainFile → executeLines); the passthrough ships with + * the XRPLib examples, so nothing is uploaded. Once it starts, the robot + * sends VAR_DEFs for its $servo.N variables — their arrival in the + * NetworkTable is the confirmation that puppet mode is live. + */ + +/** On-robot path of the passthrough program (flashed with the XRPLib examples). */ +export const PUPPET_PASSTHROUGH_PATH = '/XRPExamples/puppet_passthrough.py'; + +/** + * Start puppet mode on the connected XRP. + * Returns false when there is no connection or the run machinery is busy; + * the caller should watch the NetworkTable for $servo.N definitions to know + * the passthrough is actually up. + */ +export async function startPuppetPassthrough(): Promise { + const connection = AppMgr.getInstance().getConnection(); + if (!connection?.isConnected()) { + return false; + } + + const cmdMgr = CommandToXRPMgr.getInstance(); + + // Interrupt any running user program (guarded no-op when idle), and give + // the interrupt/REPL a moment to settle before starting the passthrough. + cmdMgr.stopProgram(); + await new Promise((resolve) => setTimeout(resolve, 500)); + + const lines = await cmdMgr.updateMainFile(PUPPET_PASSTHROUGH_PATH); + if (!lines) { + return false; // run machinery busy + } + await cmdMgr.executeLines(lines); + return true; +} + +/** Stop puppet mode — identical to pressing STOP on a running program. */ +export function stopPuppetPassthrough(): void { + CommandToXRPMgr.getInstance().stopProgram(); +} diff --git a/src/components/dashboard/utils/xppUtils.ts b/src/components/dashboard/utils/xppUtils.ts index 8e400e2..2da916c 100644 --- a/src/components/dashboard/utils/xppUtils.ts +++ b/src/components/dashboard/utils/xppUtils.ts @@ -49,7 +49,10 @@ export function buildVarUpdatePacket(meta: CustomVarMeta, value: number): Uint8A /** * Send a variable update to the XRP. - * Uses the active connection's writeToDevice. + * Uses the active connection's writeToDataDevice: over BLE that is the + * dedicated binary DATA characteristic (the only input the robot's puppet + * protocol listens to on BLE); over USB it falls through to the normal + * serial stream, which the puppet polls directly. * Returns true if sent, false if no connection available. */ export async function sendVariableUpdate(meta: CustomVarMeta, value: number): Promise { @@ -61,7 +64,7 @@ export async function sendVariableUpdate(meta: CustomVarMeta, value: number): Pr const packet = buildVarUpdatePacket(meta, value); try { - await connection.writeToDevice(packet); + await connection.writeToDataDevice(packet); return true; } catch (err) { console.error('Error sending variable update:', err); diff --git a/src/components/dashboard/xrp-dashboard.tsx b/src/components/dashboard/xrp-dashboard.tsx index b70fcdb..bdef55a 100644 --- a/src/components/dashboard/xrp-dashboard.tsx +++ b/src/components/dashboard/xrp-dashboard.tsx @@ -10,6 +10,7 @@ import { } from "./sensors"; import CustomXPPSensor from "./sensors/CustomXPPSensor"; import CustomVariableWidget from "./sensors/CustomVariableWidget"; +import PuppetControlWidget from "./sensors/PuppetControlWidget"; import { getCustomSensor } from "./sensors/customRegistry"; import { GridStackOptions } from "gridstack"; import AddWidgets from "./AddWidget"; @@ -69,6 +70,8 @@ const COMPONENT_MAP = { CustomVariable: ({ initialVarName }: { initialVarName?: string }) => ( ), + + PuppetControl: () => , }; const gridOptions: GridStackOptions = { diff --git a/src/connections/bluetoothconnection.ts b/src/connections/bluetoothconnection.ts index a072998..d44bdf9 100644 --- a/src/connections/bluetoothconnection.ts +++ b/src/connections/bluetoothconnection.ts @@ -440,21 +440,24 @@ export class BluetoothConnection extends Connection { } /** - * writeToDataDevice - * @param Uint8Array + * writeToDataDevice - write binary (XPP) data to the DATA characteristic, + * the only input the robot's puppet protocol listens to over BLE. + * Chained on the same queue as the REPL writes: GATT allows only one + * operation at a time per device, so an unqueued write here would race + * shell/REPL traffic and get dropped with "operation already in progress". + * @param data */ public async writeToDataDevice(data: Uint8Array) { this.connLogger.debug('writeToDataDevice BLE: ' + data); - try { - //this.connLogger.debug("writing: " + this.TEXT_DECODER.decode(str)); - await this.bleDataWriter?.writeValue(data as BufferSource); - - } catch (error) { - this.connLogger.debug(error); - } - - return Promise.resolve(); // Indicate success + this.Queue = this.Queue.then(async () => { + try { + await this.bleDataWriter?.writeValue(data as BufferSource); + } catch (error) { + console.error('ble data write failed:', error); + } + }); + return this.Queue; } /** diff --git a/src/connections/connection.ts b/src/connections/connection.ts index d9b06f4..d09e011 100644 --- a/src/connections/connection.ts +++ b/src/connections/connection.ts @@ -480,6 +480,19 @@ abstract class Connection { this.connLogger.debug('Writing to device' + str); } + /** + * writeToDataDevice - write binary (XPP) data to the device. + * Transports with a dedicated binary channel override this: over BLE + * the robot only feeds XPP from the DATA characteristic (the REPL + * characteristic is dupterm'd into stdin, where XPP bytes would be + * lost). Over USB there is a single stream, so the default of writing + * to the normal device stream is correct. + * @param data + */ + public async writeToDataDevice(data: Uint8Array) { + await this.writeToDevice(data); + } + // Goes into raw mode and writes a command according to the XRP_SEND_BLOCK_SIZE then executes public async writeUtilityCmdRaw( cmdStr: string,