commit e8d61a1c98addf8bdf3371c3b08d3d75583478e8 Author: Carl Vargklint Date: Sun Sep 20 18:55:16 2026 +0200 First commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..34c24f6 --- /dev/null +++ b/README.md @@ -0,0 +1,297 @@ +# TranspOtter + +A hoverboard mainboard driven as a differential-drive robot base, with an +ESP32-C3 Supermini as the motion controller. + +| Folder | What it is | +| --- | --- | +| `hoverboard-firmware-hack-FOC/` | The STM32 mainboard firmware ([EFeru](https://github.com/EFeru/hoverboard-firmware-hack-FOC)), configured here for heavy loads and programmatic moves | +| `esp32/` | PlatformIO project for the ESP32-C3 Supermini: jerk-limited motion planning, speed-dependent steering, closed-loop distance and rotation | + +The mainboard runs closed-loop speed control on each wheel. The ESP32 decides +*what* the wheels should do and shapes every change in command so a loaded +chassis never lurches. + +--- + +## Wiring + +The ESP32 hangs off the **left sideboard cable** (USART2, the long one). Both +ends are 3.3 V logic, so no level shifter is needed — just three signal-level +connections and a regulator for power. Note that PA2/PA3 are **not** 5 V +tolerant: nothing running at 5 V may touch these two wires. + +``` + Hoverboard mainboard ESP32-C3 Supermini + left sideboard cable (4-pin) + ┌──────────────────────────┐ + │ red 15 V, 200 mA max │──► buck 15V→5V ──► 5V pin + │ blue PA2 / USART2 TX │───────────────────► GPIO20 (ESP32 RX) + │ green PA3 / USART2 RX │◄────────────────── GPIO10 (ESP32 TX) + │ black GND │───────────────────► GND (also buck GND) + └──────────────────────────┘ +``` + +| Mainboard | Wire | MCU pin | ESP32-C3 | Note | +| --- | --- | --- | --- | --- | +| USART2 TX | blue | PA2 | GPIO20 | mainboard talks, ESP32 listens | +| USART2 RX | green | PA3 | GPIO10 | ESP32 talks, mainboard listens | +| GND | black | — | GND | **must** be common, connect this first | +| 15 V | red | — | 5V *via buck converter* | 200 mA max; never straight to the ESP32 | + +Cross the two data wires: TX goes to RX. Pin numbering and wire colours are from +`hoverboard-firmware-hack-FOC/docs/pictures/mainboard_pinout.png`. + +GPIO20 and GPIO10 are adjacent on the Supermini header and neither is a +strapping pin. Steer clear of GPIO2, GPIO8 and GPIO9 for this link — they +decide boot mode, GPIO9 is wired to the BOOT button (which would short the +mainboard's TX line every time you flash), and GPIO8 drives the onboard LED. +GPIO21 is left free on purpose: the ROM bootloader prints its log there at +every reset. + +### Power + +The 15 V rail on the sideboard cable cannot feed the Supermini directly — its +5V pin expects 5 V. Put a small buck converter (MP1584, LM2596 or similar) in +between and set it to 5.0 V **before** connecting it. The ESP32-C3 peaks around +350 mA at 3.3 V, which is roughly 90 mA off the 15 V rail, comfortably inside +the 200 mA budget. + +Three things worth getting right: + +- **Do not power from USB and the buck at the same time.** Most Supermini + boards have no reverse-current protection on the 5V pin. Either unplug the + buck while flashing over USB, or fit a Schottky diode in series with the buck + output. +- **The 15 V rail is only live when the mainboard is on.** The ESP32 loses + power when the board latches off (power button, low battery, inactivity + timeout). That is intentional: no mainboard, no controller. +- **Never feed 3.3 V or 5 V into the mainboard** from the ESP32 side. + +### Choosing the other cable instead + +To move the link to the right cable (USART3, PB10 = TX, PB11 = RX): swap the two +`CONTROL_SERIAL_USART2` / `FEEDBACK_SERIAL_USART2` lines in the firmware's +`Inc/config.h` for their USART3 equivalents and rebuild. The right cable is 5 V +tolerant and shorter; the left cable is longer and leaves USART3 free, which is +the only port that can do I²C if a character LCD is ever wanted. + +**Which cable the firmware listens on and which one you physically wire must +match.** They fail in a distinctive way if they don't: the mainboard is both +deaf and mute, so you get three repeating low beeps (its serial-timeout code) +*and* no feedback bytes at the ESP32. A single broken wire kills only one +direction; losing both at once means wrong cable, swapped blue/green, or no +common ground. + +--- + +## Mainboard firmware configuration + +Configured in `hoverboard-firmware-hack-FOC/Inc/config.h`. Every deviation from +upstream is tagged `[robot]`, so `grep -n '\[robot\]' Inc/config.h` lists them. + +| Setting | Value | Why | +| --- | --- | --- | +| `VARIANT_USART` | enabled | the ESP32 drives the board over UART | +| `CONTROL_SERIAL_USART2` / `FEEDBACK_SERIAL_USART2` | enabled | link on the left cable, bidirectional | +| `TANK_STEERING` | enabled | per-wheel commands; the ESP32 owns the kinematics | +| `CTRL_MOD_REQ` | `SPD_MODE` | closed-loop rpm — holds commanded speed under load | +| `N_MOT_MAX` | 200 rpm | ≈1.7 m/s on 6.5" wheels; low ceiling buys resolution and torque margin | +| `I_MOT_MAX` / `I_DC_MAX` | 20 A / 22 A | headroom for heavy loads and hill starts | +| `ENABLE_ODOMETRY` | enabled | hall tick counters in the feedback frame | +| `RATE` | 160 | on-board ramp limit, 2000 command units/s — a backstop under the ESP32's ramp | +| `INACTIVITY_TIMEOUT` | 30 min | a robot idling between jobs should not switch itself off | +| `BEEPS_BACKWARD` | 0 | programmatic reversing should not beep; fault beeps still work | + +**Why speed mode.** In `VLT_MODE` a command sets motor voltage, so the same +command gives a different speed empty than loaded, and a slope changes it +again. `SPD_MODE` closes the loop on measured rpm: the commanded speed is the +speed you get, which is what makes a distance or rotation command repeatable. +The trade is that the wheels actively hold zero — the robot resists being +pushed, and a stalled wheel will pull up to `I_MOT_MAX` continuously. Keep an +eye on `boardTemp` under sustained load. + +### Build and flash + +```bash +cd hoverboard-firmware-hack-FOC +./build.sh # ~34 KB, uses the variant set in Inc/config.h +./flash.sh -u # first flash of a stock board only: clears read-out protection +./flash.sh -b # build and flash +``` + +Power the board from its battery and **hold the power button** for the whole +operation, or feed 3.3 V from the ST-Link with the battery disconnected. An +erased chip cannot hold its own power latch, so the board switches itself off +the moment you let go of the button. + +### GigaDevice GD32F103 mainboards + +Some boards carry a GD32F103 instead of the STM32. Same firmware, same wiring: + +```bash +./flash-gd32.sh -b # or: ./flash.sh -c gd32 -b +./flash-gd32.sh -s 0x20000 # if yours is a 128 KB part +``` + +There is no `gd32f1x.cfg` in OpenOCD; the GD32 is programmed through the same +`stm32f1x` driver. The one thing that reliably goes wrong is flash size +detection — GD32 parts often report the wrong size, and OpenOCD then refuses to +write past the size it believes in. The GD32 path states the size outright +rather than asking: 256 KB by default, which is the RCT6 these boards carry. +`st-flash` has no equivalent override, so GD32 always goes via OpenOCD. + +--- + +## ESP32 controller + +```bash +cd esp32 +pio run # build +pio run -t upload # flash over USB-C +pio device monitor # console at 115200 +``` + +### What it does + +**Jerk-limited motion.** Linear and angular velocity are each planned with a +limit on acceleration *and* on how fast acceleration itself may change. A +command step becomes an S-curve, so a loaded chassis starts and stops without +the forward-backward rock you get from a step in torque. Tune with +`A_MAX_MPS2`, `J_MAX_MPS3`, `ALPHA_MAX` and `J_MAX_ALPHA` in +`include/robot_config.h`; lower jerk means gentler, slower transitions. + +**Speed-dependent steering.** Angular authority is scaled by + +``` +gain(s) = STEER_GAIN_HIGH + (STEER_GAIN_LOW - STEER_GAIN_HIGH) · e^(-s / STEER_KNEE) +``` + +where `s` is the current speed as a fraction of maximum. With the defaults, a +full steer deflection spins on the spot at a standstill (gain 1.00) and becomes +a gentle curve at full speed (gain 0.22), with most of the change happening +inside the first 15% of the speed range. Raise `STEER_KNEE` to spread the +transition over a wider speed band; lower `STEER_GAIN_HIGH` for an even calmer +feel at speed. The gain is evaluated against actual speed every tick, so it +fades in and out as the robot accelerates. + +**Closed-loop moves.** `fwd` and `rot` count hall ticks from the feedback frame +(5.76 mm of travel per tick on 6.5" wheels) and brake on a profile that accounts +for the jerk ramp, so they stop on the mark instead of sailing past it. A +straight move holds its starting heading; a rotation holds station. Both finish +inside ~10 mm / ~1°. + +### Console + +| Command | Effect | +| --- | --- | +| `drive ` | −1..1 each; `steer` goes through the speed curve | +| `vel ` | physical units, no steering curve — the programmatic path | +| `fwd ` | closed-loop straight move | +| `rot ` | closed-loop spin on the spot, + is left | +| `stop` | ramp down through the jerk limits | +| `estop` | zero the command immediately, then latch until `arm` | +| `zero` | reset the odometry accumulators | +| `speed <0..1>` | joystick throttle scale, saved to flash | +| `resp <0..1>` | acceleration sharpness, saved to flash | +| `tel [on\|off]`, `status` | telemetry stream / one-shot dump | + +`drive` and `vel` are standing orders with a 2 s deadman: refresh them or the +controller ramps down. `fwd` and `rot` end by arriving. + +--- + +## Remote control + +The ESP32 hosts its own WiFi network and serves the control page from flash — +no router, no app, nothing to fetch. + +1. Join **`transpotter`**, password **`otterdrive`** (both in `robot_config.h`) +2. Open **http://192.168.4.1** + +The page gives a touch joystick, a speed limit, a response slider with three +presets, STOP / E-STOP / ARM / ZERO, and live telemetry at 10 Hz. + +The browser sends a drive frame every 100 ms whether or not the stick moved, +and the firmware answers each one with telemetry — so the exchange doubles as a +heartbeat. Background the tab or walk out of range and `REMOTE_DEADMAN_MS` +(400 ms) ramps the machine down. The console keeps a relaxed 2 s deadman, since +nobody types at 5 Hz. The speed limit is applied on the device rather than in +the page, so a stale browser tab cannot ask for more than the saved limit. + +Range is the weak point: the Supermini's onboard antenna measured a signal of +54 against a 27–100 spread of local networks. Fine across a room. If you need +more, a C3 module with a u.FL connector beats any firmware change. + +### Response: acceleration sharpness + +One setting scales acceleration and jerk on both axes between two tunings, live +and saved to flash. Top speed is separate — `speed` caps how fast, `response` +governs how hard it gets there. + +| | Acceleration | 0 → 0.8 m/s | Use | +| --- | --- | --- | --- | +| `HAUL` (0.00) | 0.50 m/s² | 1.75 s | Long distances at walking pace, loaded | +| `NORMAL` (0.45) | ~0.95 m/s² | ~1.0 s | General purpose | +| `INDOOR` (1.00) | 1.50 m/s² | 0.60 s | Tight, reactive spaces | + +Both ends are measured on hardware, not calculated. The bounds are +`A_MAX_MPS2`/`J_MAX_MPS3` (soft) and `A_MAX_SHARP`/`J_MAX_SHARP` (sharp) in +`robot_config.h`; the closed-loop braking profile reads the live values, so +`fwd` and `rot` stop correctly at any response setting. + +Settings are written to NVS 1.5 s after the last change — dragging a slider +does not hammer the flash. + +## Bring-up + +**Put the chassis on blocks with both wheels off the ground for steps 1–4.** + +1. Flash both halves. Power the mainboard from its battery, press the power + button, and open the ESP32 console. It holds a zero command for 1.5 s so the + mainboard arms its motors, then prints `armed`. +2. `status` — `link=1` and a sane battery voltage mean the serial link is good + in both directions. If `link=0`, the two data wires are probably not crossed. +3. `vel 0.2 0` — both wheels should turn *forward*. If one runs backwards, set + `INVERT_LEFT` or `INVERT_RIGHT` in `include/robot_config.h` and reflash. +4. `zero`, `vel 0.2 0` for a second, `stop`, then `status` — `dist` should be + positive and `head` near zero. A negative distance means both odometry + channels are inverted; a large heading means one is. Fix with + `INVERT_ODOM_LEFT` / `INVERT_ODOM_RIGHT`. +5. Measure the distance between the wheel contact patches and set + `TRACK_WIDTH_M`. On the floor, `rot 360` should come back to where it + started — if it overshoots, the track width is set too small. +6. Check `fwd 1` against a tape measure and adjust `WHEEL_DIAMETER_M` if needed. + +Then raise the limits in `robot_config.h` to suit the load. + +## Protocol + +100 Hz, 115200 8N1, little-endian, XOR checksum. The mainboard cuts the motors +if no valid command arrives for 0.8 s. + +``` +command (8 bytes) uint16 0xABCD │ int16 left │ int16 right │ uint16 checksum +feedback (22 bytes) uint16 0xABCD │ int16 cmd1, cmd2, speedR, speedL, + wheelR_cnt, wheelL_cnt, batVoltage, + boardTemp │ uint16 cmdLed │ uint16 checksum +``` + +`left`/`right` are ±1000, mapping linearly to ±`N_MOT_MAX` rpm in speed mode. +The wheel counters wrap at 9000 (100 wheel revolutions), which +`hoverTickDelta()` unwraps. The feedback layout above assumes `ENABLE_ODOMETRY` +— without it the two counters are absent and the frame is four bytes shorter. + +The console grammar is the natural interface for an app to drive over BLE or +WiFi later: the same verbs, one line each. + +## Safety + +- Commission with the wheels off the ground. These motors have real torque. +- `estop` is a software stop. Fit a physical battery disconnect for anything + carrying a load. +- `I_MOT_MAX` is raised to 20 A. Watch `boardTemp` during the first loaded runs; + the mainboard's own overheat cutoff is disabled until calibrated (see the + temperature section of `Inc/config.h`). +- Raising `N_MOT_MAX` scales every speed in the system, including what a runaway + would do. diff --git a/esp32-ble/.gitignore b/esp32-ble/.gitignore new file mode 100644 index 0000000..968a41b --- /dev/null +++ b/esp32-ble/.gitignore @@ -0,0 +1,2 @@ +.pio/ +.vscode/ diff --git a/esp32-ble/console.sh b/esp32-ble/console.sh new file mode 100755 index 0000000..bfde9a3 --- /dev/null +++ b/esp32-ble/console.sh @@ -0,0 +1,154 @@ +#!/usr/bin/env bash +# +# Open the TranspOtter serial console. +# +# ./console.sh minicom, port auto-detected +# ./console.sh /dev/ttyACM1 force a port +# ./console.sh -m use `pio device monitor` instead +# ./console.sh -r raw stty/cat fallback, no terminal program +# ./console.sh -l list candidate ports and exit +# +# Exit minicom with Ctrl-A X. +set -euo pipefail + +PROFILE_NAME=transpotter +PROFILE="$HOME/.minirc.$PROFILE_NAME" +BAUD=115200 +MODE=minicom +PORT="${TRANSPOTTER_PORT:-}" + +# Print the comment block at the top of this file, stopping at the first line +# of actual code, so the help text cannot drift out of sync with it. +usage() { awk 'NR>2 && /^#/ { sub(/^# ?/, ""); print; next } NR>2 { exit }' "$0"; } + +while [ $# -gt 0 ]; do + case "$1" in + -m|--monitor) MODE=monitor ;; + -r|--raw) MODE=raw ;; + -l|--list) MODE=list ;; + -h|--help) usage; exit 0 ;; + -*) echo "unknown option: $1" >&2; usage >&2; exit 2 ;; + *) PORT="$1" ;; + esac + shift +done + +# --- find the board ---------------------------------------------------------- +# by-id first: the name is stable across replugs and unambiguous when several +# USB serial devices are attached. Falls back to matching Espressif's USB vendor +# id (0x303a) in sysfs, then to the usual device node. +list_ports() { + local found=0 + for l in /dev/serial/by-id/*Espressif*; do + [ -e "$l" ] && { echo "$l"; found=1; } + done + for d in /sys/class/tty/ttyACM* /sys/class/tty/ttyUSB*; do + [ -e "$d" ] || continue + [ "$(cat "$d/device/../idVendor" 2>/dev/null)" = "303a" ] || continue + echo "/dev/$(basename "$d")"; found=1 + done + [ "$found" -eq 0 ] && [ -e /dev/ttyACM0 ] && echo /dev/ttyACM0 + return 0 +} + +if [ "$MODE" = list ]; then + echo "candidate ports:"; list_ports | sort -u | sed 's/^/ /' + exit 0 +fi + +if [ -z "$PORT" ]; then + PORT="$(list_ports | head -1 || true)" +fi + +if [ -z "$PORT" ] || [ ! -e "$PORT" ]; then + echo "error: no serial port found." >&2 + echo " Plug the ESP32 in, or pass one explicitly: $0 /dev/ttyACM0" >&2 + echo " '$0 -l' lists candidates." >&2 + exit 1 +fi + +if [ ! -w "$PORT" ]; then + echo "error: $PORT is not writable. You likely need to be in the 'plugdev'" >&2 + echo " or 'dialout' group: sudo usermod -aG plugdev \$USER (then re-login)." >&2 + exit 1 +fi + +# A second reader steals half the bytes and makes the console look broken — and +# blocks `pio run -t upload` outright. fuser/lsof are often not installed, so +# walk /proc instead: our own processes' fds are always readable. +port_holders() { + local target pid + target="$(readlink -f "$1" 2>/dev/null)" || return 0 + for l in /proc/[0-9]*/fd/*; do + [ -e "$l" ] || continue + [ "$(readlink -f "$l" 2>/dev/null)" = "$target" ] || continue + pid="${l#/proc/}"; pid="${pid%%/*}" + [ "$pid" = "$$" ] && continue + echo "$pid $(tr '\0' ' ' < "/proc/$pid/cmdline" 2>/dev/null)" + done | sort -u +} + +HOLDERS="$(port_holders "$PORT")" +if [ -n "$HOLDERS" ]; then + echo "warning: something else already has $PORT open:" >&2 + printf '%s\n' "$HOLDERS" | sed 's/^/ /' >&2 + echo " close it first — two readers split the bytes between them," >&2 + echo " and an upload cannot open the port at all." >&2 +fi + +# --- alternate modes --------------------------------------------------------- +case "$MODE" in + monitor) + exec pio device monitor -p "$PORT" -b "$BAUD" + ;; + raw) + echo "raw mode on $PORT — type a command and press Enter, Ctrl-C to quit." + stty -F "$PORT" "$BAUD" raw -echo -crtscts + cat "$PORT" & + CATPID=$! + trap 'kill $CATPID 2>/dev/null || true' EXIT + while IFS= read -r line; do printf '%s\n' "$line" > "$PORT"; done + exit 0 + ;; +esac + +# --- minicom ----------------------------------------------------------------- +command -v minicom >/dev/null 2>&1 || { + echo "error: minicom is not installed. Try '$0 -m' (pio device monitor)" >&2 + echo " or '$0 -r' (raw), or: sudo apt install minicom" >&2 + exit 1 +} + +# Three minicom defaults make this board awkward, so the profile is written +# rather than assumed: +# rtscts No - the C3's USB CDC never asserts CTS, and minicom defaults +# hardware flow control ON, silently blocking every keystroke. +# localecho - the firmware does not echo, so without this nothing appears +# as you type and the terminal seems unresponsive. +# addcarreturn - turns a bare LF into CR+LF on the way in. Firmware that +# prints "\n" without "\r" otherwise staircases off the right +# of the screen. Harmless when the firmware already sends +# CR+LF, so it is on regardless. +WANT=$(cat < "$PROFILE" + echo "wrote $PROFILE" +fi + +echo "connecting to $PORT at $BAUD (Ctrl-A X to quit, Ctrl-A Z for help)" +echo "note: opening the port resets the ESP32 — expect the boot banner." +exec minicom "$PROFILE_NAME" -D "$PORT" diff --git a/esp32-ble/include/ble_pad.h b/esp32-ble/include/ble_pad.h new file mode 100644 index 0000000..9942feb --- /dev/null +++ b/esp32-ble/include/ble_pad.h @@ -0,0 +1,134 @@ +// BLE HID gamepad client. +// +// The ESP32-C3 has no Bluetooth Classic, so this only works with controllers +// that speak BLE HID (HOGP) — 8BitDo in its BLE modes, Xbox Series pads, and +// most cheap "BLE gamepad" units. PS4/PS5/Switch Pro are Classic only and will +// not appear at all. +// +// NimBLE ships no HID *client*, so this talks raw GATT: find service 0x1812, +// subscribe to every notifying report characteristic, and hand the raw bytes +// up. Report layouts differ wildly between pads, so instead of parsing HID +// report descriptors there is a learn mode: press a button and the changed +// bits become the binding. +#pragma once + +#include +#include + +namespace blepad { + +enum Fn : uint8_t { FN_FWD, FN_BACK, FN_LEFT, FN_RIGHT, FN_BRAKE, FN_TURBO, FN_STOP, FN_COUNT }; +extern const char* const FN_NAME[FN_COUNT]; + +// A button is "active" when (report[index] & mask) == value. +// +// A single-bit change gives a bit mask, which combines with other buttons in +// the same byte. A multi-bit change — a D-pad hat, where centre might be 0x0F +// and up 0x00 — gives an exact byte match instead, since those values are +// mutually exclusive rather than independent bits. +struct Binding { + bool set = false; + uint8_t index = 0; + uint8_t mask = 0; + uint8_t value = 0; +}; + +// An analog axis: one byte sweeping between two extremes through a centre. +// Covers a stick, a trigger, and the common case of a single byte encoding +// both directions of one axis (up/down or left/right). +enum Axis : uint8_t { AX_THROTTLE, AX_STEER, AX_COUNT }; +extern const char* const AX_NAME[AX_COUNT]; + +struct AxisBinding { + bool set = false; + uint8_t index = 0; + uint8_t centre = 128; + uint8_t lo = 0; + uint8_t hi = 255; + uint8_t deadband = 6; // counts either side of centre that read as zero + bool invert = false; +}; + +struct Found { + char name[24] = {0}; + NimBLEAddress address; + int8_t rssi = 0; + bool hid = false; +}; + +void begin(); + +// Scanning and connecting both take seconds and both are run on a worker task. +// Doing them inline would stall the 100 Hz control loop well past the +// mainboard's 0.8 s serial timeout, which disarms the motors and beeps. +void requestScan(uint32_t seconds); +bool busy(); // a scan or connect attempt is in flight + +size_t foundCount(); +const Found& found(size_t i); + +// Ask the worker to connect. It keeps retrying with backoff until it succeeds, +// the target is cleared, or auto-reconnect is turned off. +bool requestConnectIndex(size_t i); +bool requestConnectAddress(const char* addr); + +void disconnect(); // drop the link AND stop retrying +bool connected(); +void forgetBonds(); + +// The bond store remembers the paired controller across reboots, so the +// worker can re-establish the link with no console involvement. +bool hasTarget(); +const char* targetString(); +void setAutoReconnect(bool on); +bool autoReconnect(); + +// Most recent HID report, for the terminal's raw dump. +bool latestReport(uint8_t* buf, size_t cap, size_t& len); +uint32_t reportCount(); + +// Level test against the newest report. Only meaningful for controllers that +// report button release; see takeReportEvent() for the ones that do not. +bool bindingActive(Fn f); + +// True once per newly arrived report. A controller that only transmits on +// press leaves its last report showing that button held forever, so the +// arrival of a report — not its contents — is the click. The counter moves +// even when two reports are byte-identical, so repeated presses of the same +// button are each seen. +bool takeReportEvent(uint8_t* buf, size_t cap, size_t& len); + +// Test a binding against a specific report rather than the latest one. +bool matches(const Binding& b, const uint8_t* rep, size_t len); +// Learning samples over a window rather than snapping to the first report, so +// it can see whether a byte toggles like a button or sweeps like an axis. +// `exact` forces a whole-byte match instead of inferring a bit mask. +void startLearn(Fn f, bool exact = false, uint32_t windowMs = 1500); +void startAxisLearn(Axis a, uint32_t windowMs = 4000); + +// Drives the learn window. Call once per control tick. +void poll(uint32_t nowMs); + +// Axis reading in -1..1, already centred, scaled, deadbanded and inverted. +// Returns 0 when unmapped or when the report is stale. +float axisValue(Axis a, bool stale); +const AxisBinding& axis(Axis a); +void setAxis(Axis a, const AxisBinding& b); +bool takeLearnedAxis(Axis& a, AxisBinding& b); +void clearAxes(); + +// Feed a synthetic report through the same path a real one takes. For testing +// a mapping, and for working on this without a controller to hand. +void injectReport(const uint8_t* data, size_t len); +bool learning(); +Fn learnTarget(); +bool takeLearned(Fn& f, Binding& b); // true once, when a press was captured + +const Binding& binding(Fn f); +void setBinding(Fn f, const Binding& b); +void clearBindings(); + +// Edge-triggered connection events, for beeping and logging. +bool takeConnectEvent(bool& nowConnected); + +} // namespace blepad diff --git a/esp32-ble/include/hoverboard.h b/esp32-ble/include/hoverboard.h new file mode 100644 index 0000000..7858acd --- /dev/null +++ b/esp32-ble/include/hoverboard.h @@ -0,0 +1,89 @@ +// Serial link to a hoverboard mainboard running hoverboard-firmware-hack-FOC. +// +// Frames are the firmware's own structs (Inc/util.h, Src/main.c) — all int16, +// naturally packed, little-endian on both ends, XOR checksum. +#pragma once + +#include +#include "robot_config.h" + +constexpr uint16_t HOVER_START_FRAME = 0xABCD; // firmware SERIAL_START_FRAME + +// What we send. Under TANK_STEERING the firmware assigns: +// command.steer -> cmdL (left wheel), command.speed -> cmdR (right wheel). +// The field names below say what they actually drive. +struct __attribute__((packed)) HoverCommand { + uint16_t start; + int16_t left; + int16_t right; + // Mainboard buzzer: low byte = pitch (0 = silent), high byte = pattern. + // Level-triggered, so we own the duration; the mainboard's own fault beeps + // take priority over anything requested here. + uint16_t buzzer; + uint16_t checksum; +}; + +// What we receive. This layout assumes ENABLE_ODOMETRY is defined in the +// firmware; without it the two wheel counters are absent and the frame is four +// bytes shorter, which shows up here as a permanent checksum failure. +struct __attribute__((packed)) HoverFeedback { + uint16_t start; + int16_t cmd1; // command echo, left + int16_t cmd2; // command echo, right + int16_t speedR_meas; // rpm + int16_t speedL_meas; // rpm + int16_t wheelR_cnt; // hall ticks, wraps at ODOM_MODULO + int16_t wheelL_cnt; + int16_t batVoltage; // volts * 100 + int16_t boardTemp; // degrees C * 10 + uint16_t cmdLed; + uint16_t checksum; +}; + +class HoverboardLink { +public: + void begin(HardwareSerial& port); + + // Queue a command. Values are clamped to +/-CMD_MAX and wheel inversion from + // robot_config.h is applied here, so callers work in robot coordinates. + void send(int16_t left, int16_t right); + + // Sound the mainboard buzzer for `ms`, then clear it automatically. Pitch + // and pattern use the firmware's own scale: lower pitch value = higher tone, + // pattern 0 = continuous. Non-blocking. + void beep(uint8_t pitch, uint8_t pattern = 0, uint16_t ms = 120); + void beepOff(); + + // Drain the receive buffer. Returns true if a valid frame arrived this call. + bool poll(); + + const HoverFeedback& feedback() const { return fb_; } + uint32_t lastFrameMs() const { return lastFrameMs_; } + bool linkUp() const; + uint32_t framesOk() const { return framesOk_; } + uint32_t framesBad() const { return framesBad_; } + // Raw bytes seen on RX. Separates "the wire is silent" from "bytes are + // arriving but never frame up" (wrong baud, swapped pins, wrong layout). + uint32_t rxBytes() const { return rxBytes_; } + + float batteryVolts() const { return fb_.batVoltage / 100.0f; } + float boardTempC() const { return fb_.boardTemp / 10.0f; } + +private: + HardwareSerial* port_ = nullptr; + HoverFeedback fb_{}; + uint8_t buf_[sizeof(HoverFeedback)]{}; + size_t have_ = 0; + uint32_t lastFrameMs_ = 0; + uint32_t framesOk_ = 0; + uint32_t framesBad_ = 0; + uint32_t rxBytes_ = 0; + uint16_t buzzer_ = 0; + uint32_t beepUntilMs_ = 0; + + void dropByte(); +}; + +// Signed difference between two readings of a counter that wraps at +// ODOM_MODULO, taking the short way round. +int16_t hoverTickDelta(int16_t prev, int16_t curr); diff --git a/esp32-ble/include/motion.h b/esp32-ble/include/motion.h new file mode 100644 index 0000000..a0cf3ad --- /dev/null +++ b/esp32-ble/include/motion.h @@ -0,0 +1,121 @@ +// Motion planning: jerk-limited body velocities, a speed-dependent steering +// curve, and closed-loop distance / rotation moves driven by wheel odometry. +#pragma once + +#include +#include "robot_config.h" + +// Wheel travel accumulated from the firmware's hall tick counters. +class Odometry { +public: + void reset(); // zero the accumulators + void update(int16_t rawLeft, int16_t rawRight); + + double left() const { return left_m_; } + double right() const { return right_m_; } + double distance() const { return 0.5 * (left_m_ + right_m_); } // metres + double heading() const { return (right_m_ - left_m_) / TRACK_WIDTH_M; } // radians, CCW+ + +private: + bool started_ = false; + int16_t prevL_ = 0, prevR_ = 0; + double left_m_ = 0, right_m_ = 0; +}; + +// One axis of a jerk-limited profile: acceleration itself is rate-limited, so +// the command has no step in its second derivative and the load never lurches. +class JerkAxis { +public: + JerkAxis(float vmax, float amax, float jmax) : vmax_(vmax), amax_(amax), jmax_(jmax) {} + + void update(float target, float dt); + void hardZero() { v_ = 0; a_ = 0; } + void setVMax(float vmax) { vmax_ = vmax; } + void setLimits(float amax, float jmax, float amaxDec, float jmaxDec) { + amax_ = amax; jmax_ = jmax; amaxDec_ = amaxDec; jmaxDec_ = jmaxDec; + } + float velocity() const { return v_; } + float accel() const { return a_; } + +private: + float v_ = 0, a_ = 0; + float vmax_, amax_, jmax_; + float amaxDec_ = 0, jmaxDec_ = 0; // 0 until setLimits: fall back to accel +}; + +class Motion { +public: + enum class Mode { Idle, Drive, Velocity, MoveLinear, MoveAngular, EStop }; + + // Pushes the default limits into both axes. Without this the axes start with + // no braking limits set and fall back to the acceleration ones, while the + // move planner already assumes the boost — it then brakes too late. + Motion(); + + // Operator-style input, -1..1 each. `steer` is scaled by the speed-dependent + // curve, so the same stick deflection spins on the spot at a standstill and + // only leans the robot at speed. + void setDrive(float throttle, float steer); + + // Programmatic input in physical units, no steering curve applied. + void setVelocity(float mps, float radps); + + // Closed-loop moves. Both return to Idle on arrival; busy() reports progress. + void moveDistance(float metres, const Odometry& odo); + void rotate(float radians, const Odometry& odo); + + // 0 = soft (long hauls), 1 = sharp (indoors). Scales acceleration and jerk + // on both axes; top speed is a separate setting. + void setResponse(float r); + float response() const { return response_; } + + // How much firmer braking is than acceleration. 1.0 = symmetric. + void setDecelBoost(float x); + float decelBoost() const { return decelBoost_; } + + void stop(); // ramp to zero through the jerk limits + void emergencyStop(); // zero the command this instant, then latch + void clearEStop(); + + void update(const Odometry& odo, float dt); + + // True once per abandoned move, so the caller can report it. + bool takeAbort() { const bool a = aborted_; aborted_ = false; return a; } + + int16_t cmdLeft() const { return cmdL_; } + int16_t cmdRight() const { return cmdR_; } + + Mode mode() const { return mode_; } + const char* modeName() const; + bool busy() const { return mode_ == Mode::MoveLinear || mode_ == Mode::MoveAngular; } + float linear() const { return lin_.velocity(); } + float angular() const { return ang_.velocity(); } + float steerGain() const; + double remaining() const { return remaining_; } + +private: + JerkAxis lin_{V_MAX, A_MAX_MPS2, J_MAX_MPS3}; + JerkAxis ang_{W_MAX_RADS, ALPHA_MAX, J_MAX_ALPHA}; + + Mode mode_ = Mode::Idle; + float reqThrottle_ = 0, reqSteer_ = 0; // Drive mode, normalised + float reqV_ = 0, reqW_ = 0; // Velocity mode, physical units + double moveFrom_ = 0, moveTo_ = 0; // metres or radians + double holdRef_ = 0; // heading (linear move) or distance (rotation) + double remaining_ = 0; + double lastRemaining_ = 0; // for stall detection + float stallTimer_ = 0; + bool aborted_ = false; + int moveDir_ = 1; // sign of the move as commanded + + float response_ = RESPONSE_DEFAULT; + float decelBoost_ = DECEL_BOOST_DEFAULT; + float aMax_ = A_MAX_MPS2, jMax_ = J_MAX_MPS3; + float alphaMax_ = ALPHA_MAX, jAlpha_ = J_MAX_ALPHA; + + int16_t cmdL_ = 0, cmdR_ = 0; + + void mixToWheels(); + bool stalled(float dt); + bool overshot() const; +}; diff --git a/esp32-ble/include/robot_config.h b/esp32-ble/include/robot_config.h new file mode 100644 index 0000000..18f9dd1 --- /dev/null +++ b/esp32-ble/include/robot_config.h @@ -0,0 +1,182 @@ +// Tuning and wiring for the TranspOtter drive controller. +// +// Everything a new chassis needs changing is in this file. The values marked +// MEASURE are geometry — the controller's distance and rotation commands are +// only as accurate as these. +#pragma once + +#include + +// ---------------------------------------------------------------- wiring --- +// UART1 to the hoverboard's RIGHT sideboard cable (USART3). +// +// GPIO2/8/9 are strapping pins on the C3 and GPIO8 also drives the Supermini's +// LED, so all three are out. GPIO20 is UART0's default RX, but the GPIO matrix +// routes UART1 here happily — just never call Serial0.begin(), which would +// claim GPIO20/21 and fight for the pin. GPIO21 is deliberately left alone: +// the ROM bootloader logs there at every reset, which would otherwise land in +// the mainboard's receiver. +constexpr int PIN_HOVER_RX = 20; // ESP32 RX <-- mainboard TX (blue, PB10) +constexpr int PIN_HOVER_TX = 10; // ESP32 TX --> mainboard RX (green, PB11) +constexpr uint32_t HOVER_BAUD = 115200; // must match USART3_BAUD in the firmware + +// ------------------------------------------------------------- geometry --- +constexpr float WHEEL_DIAMETER_M = 0.1651f; // 6.5" hub motor. MEASURE for other wheels. +constexpr float TRACK_WIDTH_M = 0.570f; // measured, wheel centre to wheel centre +constexpr int TICKS_PER_WHEEL_REV = 90; // 15 pole pairs x 6 hall states (firmware n_polePairs) +constexpr int ODOM_MODULO = 9000; // firmware wraps wheel_cnt at 9000 (= 100 revolutions) + +// Set true if a wheel counts or drives backwards. Calibrate once, on blocks, +// with `fwd 1` and `status` — see the README. +constexpr bool INVERT_LEFT = false; +constexpr bool INVERT_RIGHT = false; +constexpr bool INVERT_ODOM_LEFT = false; +constexpr bool INVERT_ODOM_RIGHT = false; + +// ------------------------------------------- agreement with the firmware --- +// These mirror Inc/config.h in hoverboard-firmware-hack-FOC. In SPD_MODE a +// command of +/-1000 is a closed-loop request for +/-N_MOT_MAX rpm, so the +// mapping from m/s to command units is exact. Change one side, change both. +constexpr float N_MOT_MAX_RPM = 200.0f; // firmware N_MOT_MAX +constexpr int CMD_MAX = 1000; // firmware INPUT_MAX + +// -------------------------------------------------------- motion limits --- +// Body-frame limits. The linear and angular axes are each jerk-limited, so a +// wheel never sees a step in acceleration — this is what keeps a heavy load +// from lurching at the start and nodding at the stop. +constexpr float A_MAX_MPS2 = 0.50f; // linear acceleration +constexpr float J_MAX_MPS3 = 3.00f; // linear jerk: how fast acceleration itself may change. + // 1.0 was too low to stop on a mark: reversing acceleration + // took a full second, and a closed-loop move sailed 160 mm + // past its target. 3.0 still ramps in over ~170 ms, so there + // is no step in acceleration and no lurch. +// Braking may be firmer than acceleration — a gentle start is what stops a +// load lurching, but a lazy stop just feels like slack in the controls. +// +// The multiplier scales the deceleration limit by x and the deceleration JERK +// by x*x. That second part is the one you feel: the delay before braking bites +// is amax/jmax, so squaring the jerk shortens that dead time by a factor of x +// rather than leaving it unchanged. +constexpr float DECEL_BOOST_DEFAULT = 2.0f; + +constexpr float W_MAX_RADS = 1.40f; // angular rate at full steer, standstill (~80 deg/s) +constexpr float ALPHA_MAX = 1.60f; // angular acceleration +constexpr float J_MAX_ALPHA = 9.60f; // angular jerk (same reasoning as J_MAX_MPS3) + +// The four limits above are the SOFT end of the response range — the tuning +// validated on the bench, and the right feel for hauling a load a long way. +// The values below are the SHARP end, for indoor work where reacting matters +// more than smoothness. The `response` setting (0..1, live-adjustable and +// saved to flash) interpolates between the two, so nothing here is a rebuild. +constexpr float A_MAX_SHARP = 1.50f; +constexpr float J_MAX_SHARP = 9.00f; +constexpr float ALPHA_MAX_SHARP = 3.50f; +constexpr float J_ALPHA_SHARP = 21.0f; + +// Startup defaults, overridden by whatever was last saved. +constexpr float RESPONSE_DEFAULT = 0.0f; // soft +constexpr float SPEED_LIMIT_DEFAULT = 0.4f; // 40% of V_MAX on the joystick + +// --------------------------------------------------------- steering feel --- +// Angular authority as a function of how fast the robot is already going: +// +// gain(s) = HIGH + (LOW - HIGH) * exp(-s / KNEE), s = |v| / V_MAX +// +// LOW applies at standstill, decaying towards HIGH as speed builds. KNEE is the +// fraction of top speed over which most of the extra authority disappears, so a +// small KNEE means "sharp only when crawling". +constexpr float STEER_GAIN_LOW = 1.00f; // full W_MAX when stationary — spins on the spot +constexpr float STEER_GAIN_HIGH = 0.22f; // at top speed, a gentle curve instead of a swerve +constexpr float STEER_KNEE = 0.15f; // authority is ~halved by 10% of top speed + +// ---------------------------------------------------- closed-loop moves --- +constexpr float MOVE_V_MAX = 0.35f; // m/s ceiling during `fwd` (leaves braking margin) +constexpr float MOVE_W_MAX = 0.70f; // rad/s ceiling during `rot` +constexpr float MOVE_TOL_M = 0.010f; // arrival tolerance, metres (~2 hall ticks) +constexpr float MOVE_TOL_RAD = 0.020f; // arrival tolerance, radians (~1.1 deg) +constexpr float MOVE_SETTLE_MPS = 0.02f; // below this a linear move counts as stopped +constexpr float MOVE_SETTLE_RADS = 0.05f; // below this a rotation counts as stopped + +// Floor on the approach speed. Without it the braking profile decays towards +// zero as the target nears and the last millimetres take forever. Both values +// are slow enough to stop inside the tolerance above. +constexpr float MOVE_CREEP_MPS = 0.030f; +constexpr float MOVE_CREEP_RADS = 0.050f; + +// ---------------------------------------------------------------- timing --- +constexpr uint32_t CONTROL_PERIOD_MS = 10; // 100 Hz command rate (firmware times out at 800 ms) +constexpr uint32_t FEEDBACK_TIMEOUT_MS = 500; // no valid frame for this long -> ramp down +constexpr uint32_t ARM_HOLD_MS = 1500; // hold zero at boot so the firmware arms the motors + +// Derived: top wheel speed the firmware will honour, in m/s. +constexpr float PI_F = 3.14159265f; +constexpr float V_MAX = (N_MOT_MAX_RPM / 60.0f) * PI_F * WHEEL_DIAMETER_M; + +// Trim gains for closed-loop moves: a straight `fwd` holds heading, a `rot` +// holds position. Both correct the small mismatch between the two wheels. +constexpr float MOVE_HEADING_KP = 1.5f; // rad/s per radian of heading error +constexpr float MOVE_POSITION_KP = 1.0f; // m/s per metre of drift +constexpr float MOVE_TRIM_W_MAX = 0.35f; // clamp on the heading correction +constexpr float MOVE_TRIM_V_MAX = 0.15f; // clamp on the position correction + +// Metres of travel per hall tick — the resolution of every closed-loop move. +constexpr float METRES_PER_TICK = (PI_F * WHEEL_DIAMETER_M) / TICKS_PER_WHEEL_REV; + +// ------------------------------------------------------------- gamepad --- +// Turbo is a toggle on the pad. While it is on these replace the saved speed +// and response, so one button swaps a loaded walking pace for something +// reactive without touching the terminal. +constexpr float TURBO_SPEED = 0.90f; +constexpr float TURBO_RESPONSE = 0.85f; + +// Buzzer signatures, as (pitch, milliseconds). Lower pitch value = higher +// tone: the mainboard toggles the buzzer every `pitch` ticks of a 16 kHz +// counter, so 4 is about 2 kHz and 24 about 330 Hz. +constexpr uint8_t BEEP_CONNECT = 5; constexpr uint16_t BEEP_CONNECT_MS = 400; +constexpr uint8_t BEEP_DISCONNECT = 24; constexpr uint16_t BEEP_DISCONNECT_MS = 800; +constexpr uint8_t BEEP_TURBO_ON = 4; constexpr uint16_t BEEP_TURBO_ON_MS = 220; +constexpr uint8_t BEEP_TURBO_OFF = 14; constexpr uint16_t BEEP_TURBO_OFF_MS = 220; +constexpr uint8_t BEEP_BRAKE_OFF = 8; constexpr uint16_t BEEP_BRAKE_OFF_MS = 90; +constexpr uint8_t BEEP_BRAKE_ON = 16; constexpr uint16_t BEEP_BRAKE_ON_MS = 90; + +// Some remotes transmit on press only and never on release, so a button can +// never be observed going back up. For those the arrival of a report is the +// click, and each function latches or pulses rather than tracking a level. +// hold = level; needs a controller that reports release +// latch = click on, click off +// pulse = each click drives for PULSE_MS, repeated clicks extend it +constexpr uint16_t PULSE_MS_DEFAULT = 1500; + +// Controllers that repeat a report while a button is held (10-20 Hz is +// typical) need edge detection, or every repeat reads as another press. And +// because a release report can be dropped by the radio, button state is also +// treated as stale if no report arrives for this long — which doubles as a +// guard against a held direction sticking on after a lost release. +constexpr uint32_t INPUT_STALE_MS_DEFAULT = 300; + +// Latching with no release event means a lost or pocketed remote leaves the +// machine driving. In latch mode, this long without any button event clears +// the latches. 0 disables it, which is not advisable on anything heavy. +constexpr uint32_t IDLE_STOP_MS_DEFAULT = 10000; + +// A HID pad only reports when something changes, so there is no heartbeat to +// time out against. Liveness comes from the BLE link itself instead: a short +// supervision timeout means walking out of range drops the connection in about +// a second and a half rather than the default six. +constexpr uint16_t BLE_CONN_MIN_INTERVAL = 12; // x1.25 ms = 15 ms +constexpr uint16_t BLE_CONN_MAX_INTERVAL = 24; // x1.25 ms = 30 ms +constexpr uint16_t BLE_CONN_LATENCY = 0; +constexpr uint16_t BLE_CONN_TIMEOUT = 150; // x10 ms = 1.5 s + +// A closed-loop move ends by arriving, so it is exempt from the deadman below. +// That leaves it with no time limit at all, which is dangerous if the odometry +// stops advancing — a dropped link, or a wheel jammed against an obstacle. If +// the remaining distance fails to change by PROGRESS_EPS within STALL_S, the +// move is abandoned. +constexpr float MOVE_PROGRESS_EPS = 0.005f; // metres or radians +constexpr float MOVE_STALL_S = 1.5f; + +// A `drive` or `vel` command is a standing order; if nothing refreshes it +// within this long the controller ramps down. Closed-loop moves are exempt — +// they end by arriving. +constexpr uint32_t COMMAND_DEADMAN_MS = 2000; diff --git a/esp32-ble/platformio.ini b/esp32-ble/platformio.ini new file mode 100644 index 0000000..b18d515 --- /dev/null +++ b/esp32-ble/platformio.ini @@ -0,0 +1,40 @@ +; TranspOtter (BLE gamepad build) — ESP32-C3 Supermini driving a hoverboard +; mainboard running hoverboard-firmware-hack-FOC, controlled by a Bluetooth LE +; gamepad paired through a USB serial terminal. +; +; The C3 has no Bluetooth Classic, so only BLE HID controllers work here. +; +; pio run build +; pio run -t upload flash over USB-C +; pio device monitor open the console (115200) + +[env:supermini-ble] +platform = espressif32 +board = esp32-c3-devkitm-1 +framework = arduino + +monitor_speed = 115200 +monitor_filters = send_on_enter, colorize +; The C3's native USB-Serial-JTAG is not a real UART, so a baud change is both +; pointless and harmful — esptool's stub loses the link mid-handshake. 115200 is +; esptool's default, so it skips the renegotiation entirely. +upload_speed = 115200 + +; Reset over USB rather than toggling DTR/RTS like a UART bridge. Without this, +; a half-finished upload leaves the chip desynced and every later attempt hangs +; in esptool's `Connecting....` until it is physically unplugged. +board_upload.before_reset = usb_reset + +; Splices --no-stub into the esptool invocation; see the script for why. +extra_scripts = post:scripts/no_stub.py + +lib_deps = + h2zero/NimBLE-Arduino + +build_flags = + ; The Supermini's USB-C is the C3's native USB, so the console is USB CDC + ; and UART1 stays free for the hoverboard link. + -DARDUINO_USB_MODE=1 + -DARDUINO_USB_CDC_ON_BOOT=1 + -Wall + -Wextra diff --git a/esp32-ble/scripts/no_stub.py b/esp32-ble/scripts/no_stub.py new file mode 100644 index 0000000..7b9ebb3 --- /dev/null +++ b/esp32-ble/scripts/no_stub.py @@ -0,0 +1,18 @@ +# Flash the ESP32-C3 with the ROM loader instead of esptool's RAM stub. +# +# Over the C3's native USB-Serial-JTAG the stub loads and starts, then the +# transfer stalls with "Packet content transfer stopped (received 0 bytes)" — +# reproducibly so through a USB hub. The ROM loader has no such trouble and is +# no slower here, because the link runs at USB speed regardless of the nominal +# baud rate. +# +# PlatformIO exposes --before/--after through board_upload.*, but not --no-stub, +# so it gets spliced into the argument list ahead of the write_flash subcommand +# (esptool requires global options to precede it). +Import("env") + +flags = list(env.get("UPLOADERFLAGS", [])) +if "write_flash" in flags and "--no-stub" not in flags: + flags.insert(flags.index("write_flash"), "--no-stub") + env.Replace(UPLOADERFLAGS=flags) + print("no_stub.py: using the ROM loader (--no-stub) for upload") diff --git a/esp32-ble/src/ble_pad.cpp b/esp32-ble/src/ble_pad.cpp new file mode 100644 index 0000000..7b192e5 --- /dev/null +++ b/esp32-ble/src/ble_pad.cpp @@ -0,0 +1,509 @@ +#include "ble_pad.h" +#include "robot_config.h" +#include + +namespace blepad { + +const char* const FN_NAME[FN_COUNT] = {"fwd", "back", "left", "right", "brake", "turbo", "stop"}; +const char* const AX_NAME[AX_COUNT] = {"throttle", "steer"}; + +namespace { + +constexpr uint16_t HID_SERVICE = 0x1812; +constexpr size_t REPORT_MAX = 32; + +portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; + +std::vector devices; +NimBLEClient* client = nullptr; + +uint8_t report[REPORT_MAX] = {0}; +size_t reportLen = 0; +uint32_t reports = 0; + +uint32_t consumed = 0; +uint8_t idleSnapshot[REPORT_MAX] = {0}; +size_t idleLen = 0; + +enum class LearnKind : uint8_t { None, Digital, Axis }; +LearnKind learnKind = LearnKind::None; +bool learnExact = false; +Fn learnFn = FN_FWD; +Axis learnAx = AX_THROTTLE; +uint32_t learnUntil = 0; + +// Per-byte statistics gathered over the learn window. +uint8_t seenMin[REPORT_MAX]; +uint8_t seenMax[REPORT_MAX]; +uint16_t seenChanges[REPORT_MAX]; // reports where this byte differed from idle +uint8_t lastDiffValue[REPORT_MAX]; + +bool learnDone = false; +Binding learnResult{}; +bool axisLearnDone = false; +Axis axisResultFn = AX_THROTTLE; +AxisBinding axisResult{}; + +Binding bindings[FN_COUNT]; +AxisBinding axes[AX_COUNT]; + +bool connectEventPending = false; +bool connectEventState = false; + +// Worker task state. Scanning and connecting are slow, so they run here rather +// than on the control loop. +TaskHandle_t worker = nullptr; +volatile bool scanRequested = false; +volatile bool workerBusy = false; +uint32_t scanSeconds = 6; + +NimBLEAddress targetAddr; +bool haveTarget = false; +bool autoReconnectOn = true; +char targetText[20] = "none"; +uint32_t backoffMs = 2000; + +void noteConnectEvent(bool up) { + portENTER_CRITICAL(&mux); + connectEventPending = true; + connectEventState = up; + portEXIT_CRITICAL(&mux); +} + +// Accumulate per-byte statistics while a learn window is open. Deciding only +// at the end is what lets this tell a button from an axis: a button's byte +// takes one or two values, an axis sweeps across a range. +void sampleForLearn(const uint8_t* now, size_t len) { + for (size_t i = 0; i < len && i < idleLen; i++) { + if (now[i] < seenMin[i]) seenMin[i] = now[i]; + if (now[i] > seenMax[i]) seenMax[i] = now[i]; + if (now[i] != idleSnapshot[i]) { + seenChanges[i]++; + lastDiffValue[i] = now[i]; + } + } +} + +// The byte that moved most during the window. +int busiestByte() { + int best = -1; + uint32_t bestScore = 0; + for (size_t i = 0; i < idleLen; i++) { + const uint32_t range = (uint32_t)(seenMax[i] - seenMin[i]); + const uint32_t score = seenChanges[i] + range * 4; // a sweep beats a blip + if (seenChanges[i] > 0 && score > bestScore) { bestScore = score; best = (int)i; } + } + return best; +} + +void handleReport(const uint8_t* data, size_t len) { + if (len > REPORT_MAX) len = REPORT_MAX; + + portENTER_CRITICAL(&mux); + memcpy(report, data, len); + reportLen = len; + reports++; + if (learnKind != LearnKind::None) sampleForLearn(report, reportLen); + portEXIT_CRITICAL(&mux); +} + +void onNotify(NimBLERemoteCharacteristic*, uint8_t* data, size_t len, bool) { + handleReport(data, len); +} + +class ClientCallbacks : public NimBLEClientCallbacks { + void onConnect(NimBLEClient*) override { noteConnectEvent(true); } + void onDisconnect(NimBLEClient*, int) override { noteConnectEvent(false); } + void onConnectFail(NimBLEClient*, int) override { noteConnectEvent(false); } +} clientCallbacks; + +bool subscribeReports(NimBLEClient* c) { + NimBLERemoteService* hid = c->getService(NimBLEUUID(HID_SERVICE)); + if (hid == nullptr) { + Serial.println(F("! no HID service (0x1812) — this is not a BLE HID controller")); + return false; + } + + int subscribed = 0; + for (auto* ch : hid->getCharacteristics(true)) { + if (!ch->canNotify()) continue; + if (ch->subscribe(true, onNotify)) { + subscribed++; + Serial.printf(" subscribed to characteristic %s (handle %u)\r\n", + ch->getUUID().toString().c_str(), ch->getHandle()); + } + } + + if (subscribed == 0) { + Serial.println(F("! HID service has no notifying report characteristic")); + return false; + } + Serial.printf("subscribed to %d report characteristic(s)\r\n", subscribed); + return true; +} + +void doScan(uint32_t seconds) { + NimBLEScan* s = NimBLEDevice::getScan(); + s->setActiveScan(true); // ask for scan responses, which carry names + s->clearResults(); + + Serial.printf("scanning %lu s ...\r\n", (unsigned long)seconds); + NimBLEScanResults results = s->getResults(seconds * 1000, false); + + devices.clear(); + for (int i = 0; i < results.getCount(); i++) { + const NimBLEAdvertisedDevice* d = results.getDevice(i); + Found f; + const std::string n = d->getName(); + snprintf(f.name, sizeof(f.name), "%s", n.empty() ? "(unnamed)" : n.c_str()); + f.address = d->getAddress(); + f.rssi = d->getRSSI(); + f.hid = d->isAdvertisingService(NimBLEUUID(HID_SERVICE)); + devices.push_back(f); + } + s->clearResults(); +} + +bool doConnect(const NimBLEAddress& addr) { + if (client == nullptr) { + client = NimBLEDevice::createClient(); + client->setClientCallbacks(&clientCallbacks, false); + // NimBLE defaults to 30 s. A controller that is off should cost seconds + // per retry, not half a minute during which nothing else can run. + client->setConnectTimeout(5000); + } + if (client->isConnected()) client->disconnect(); + + Serial.printf("connecting to %s ...\r\n", addr.toString().c_str()); + if (!client->connect(addr)) { + Serial.println(F("! connect failed — is the pad still in pairing mode?")); + return false; + } + // Tighten the supervision timeout: a HID pad has no heartbeat of its own, so + // the link dropping is our only fast signal that the controller is gone. + client->updateConnParams(BLE_CONN_MIN_INTERVAL, BLE_CONN_MAX_INTERVAL, + BLE_CONN_LATENCY, BLE_CONN_TIMEOUT); + + if (!client->secureConnection()) { + Serial.println(F("! pairing/bonding failed")); + client->disconnect(); + return false; + } + return subscribeReports(client); +} + +void setTarget(const NimBLEAddress& a) { + targetAddr = a; + haveTarget = true; + backoffMs = 2000; + snprintf(targetText, sizeof(targetText), "%s", a.toString().c_str()); +} + +void workerLoop(void*) { + for (;;) { + if (scanRequested) { + workerBusy = true; + doScan(scanSeconds); + scanRequested = false; + Serial.printf("scan complete: %u device(s). `connect `\r\n", (unsigned)devices.size()); + for (size_t i = 0; i < devices.size(); i++) { + Serial.printf(" [%2u] %-22s %s rssi %4d %s\r\n", (unsigned)i, devices[i].name, + devices[i].address.toString().c_str(), devices[i].rssi, + devices[i].hid ? "HID" : ""); + } + workerBusy = false; + continue; + } + + const bool linkUp = client && client->isConnected(); + if (!linkUp && haveTarget && autoReconnectOn && !scanRequested) { + workerBusy = true; + const bool ok = doConnect(targetAddr); + workerBusy = false; + if (ok) { + backoffMs = 2000; + } else { + // Back off so a controller that is simply switched off does not keep + // the radio busy, but stay responsive enough to catch it powering on. + backoffMs = (backoffMs < 8000) ? backoffMs * 2 : 8000; + // Sleep in slices: a scan queued from the console must not have to + // wait out the whole backoff before it starts. + for (uint32_t waited = 0; waited < backoffMs && !scanRequested; waited += 100) { + vTaskDelay(pdMS_TO_TICKS(100)); + } + } + continue; + } + + vTaskDelay(pdMS_TO_TICKS(250)); + } +} + +} // namespace + +void begin() { + NimBLEDevice::init("transpotter"); + NimBLEDevice::setPower(9); + // HID peripherals require an encrypted link; bond so reconnects are silent. + NimBLEDevice::setSecurityAuth(true, false, true); + NimBLEDevice::setSecurityIOCap(BLE_HS_IO_NO_INPUT_OUTPUT); + + // A bond survives reboots, so a controller paired days ago is still the + // target after a power cycle — no console needed to get driving again. + if (NimBLEDevice::getNumBonds() > 0) { + setTarget(NimBLEDevice::getBondedAddress(0)); + Serial.printf("bonded controller %s — will reconnect automatically\r\n", targetText); + } + + // Priority 1 = the same as the Arduino loop task, so scanning and + // connecting time-slice with the console instead of preempting it. + xTaskCreate(workerLoop, "blework", 6144, nullptr, 1, &worker); +} + +size_t foundCount() { return devices.size(); } +const Found& found(size_t i) { return devices[i]; } + + +void requestScan(uint32_t seconds) { + scanSeconds = seconds; + scanRequested = true; +} + +bool busy() { return workerBusy || scanRequested; } + +bool requestConnectIndex(size_t i) { + if (i >= devices.size()) return false; + setTarget(devices[i].address); + return true; +} + +bool requestConnectAddress(const char* addr) { + for (size_t i = 0; i < devices.size(); i++) { + if (strcasecmp(devices[i].address.toString().c_str(), addr) == 0) { + setTarget(devices[i].address); + return true; + } + } + Serial.println(F("! address not in the scan list — run `scan` first")); + return false; +} + +void disconnect() { + // Clear the target too, or the worker reconnects within a couple of seconds. + haveTarget = false; + snprintf(targetText, sizeof(targetText), "none"); + if (client && client->isConnected()) client->disconnect(); +} + +bool connected() { return client && client->isConnected(); } + +void forgetBonds() { + disconnect(); + NimBLEDevice::deleteAllBonds(); +} + +bool hasTarget() { return haveTarget; } +const char* targetString() { return targetText; } +void setAutoReconnect(bool on) { autoReconnectOn = on; } +bool autoReconnect() { return autoReconnectOn; } + + +bool latestReport(uint8_t* buf, size_t cap, size_t& len) { + portENTER_CRITICAL(&mux); + len = reportLen < cap ? reportLen : cap; + memcpy(buf, report, len); + const bool any = reportLen > 0; + portEXIT_CRITICAL(&mux); + return any; +} + +uint32_t reportCount() { + portENTER_CRITICAL(&mux); + const uint32_t n = reports; + portEXIT_CRITICAL(&mux); + return n; +} + +bool matches(const Binding& b, const uint8_t* rep, size_t len) { + if (!b.set || b.index >= len) return false; + return (uint8_t)(rep[b.index] & b.mask) == b.value; +} + +bool takeReportEvent(uint8_t* buf, size_t cap, size_t& len) { + bool got = false; + portENTER_CRITICAL(&mux); + if (reports != consumed) { + consumed = reports; + len = reportLen < cap ? reportLen : cap; + memcpy(buf, report, len); + got = true; + } + portEXIT_CRITICAL(&mux); + return got; +} + +bool bindingActive(Fn f) { + const Binding b = bindings[f]; + if (!b.set) return false; + + bool active = false; + portENTER_CRITICAL(&mux); + if (b.index < reportLen) active = ((report[b.index] & b.mask) == b.value); + portEXIT_CRITICAL(&mux); + return active; +} + +void beginLearnWindow(uint32_t windowMs) { + portENTER_CRITICAL(&mux); + memcpy(idleSnapshot, report, reportLen); + idleLen = reportLen; + for (size_t i = 0; i < REPORT_MAX; i++) { + seenMin[i] = (i < idleLen) ? idleSnapshot[i] : 0; + seenMax[i] = (i < idleLen) ? idleSnapshot[i] : 0; + seenChanges[i] = 0; + lastDiffValue[i] = 0; + } + learnUntil = millis() + windowMs; + portEXIT_CRITICAL(&mux); +} + +void startLearn(Fn f, bool exact, uint32_t windowMs) { + learnExact = exact; + learnFn = f; + learnDone = false; + beginLearnWindow(windowMs); + learnKind = LearnKind::Digital; +} + +void startAxisLearn(Axis a, uint32_t windowMs) { + learnAx = a; + axisLearnDone = false; + beginLearnWindow(windowMs); + learnKind = LearnKind::Axis; +} + +void poll(uint32_t nowMs) { + if (learnKind == LearnKind::None) return; + if ((int32_t)(nowMs - learnUntil) < 0) return; + + const LearnKind kind = learnKind; + learnKind = LearnKind::None; + + const int b = busiestByte(); + if (b < 0) { + Serial.println(F("learn: nothing moved — no button press seen in the window")); + return; + } + + const uint8_t lo = seenMin[b], hi = seenMax[b]; + const uint8_t range = (uint8_t)(hi - lo); + Serial.printf("learn: byte %d moved %u times, values 0x%02X..0x%02X (idle 0x%02X)\r\n", + b, seenChanges[b], lo, hi, idleSnapshot[b]); + + if (kind == LearnKind::Axis) { + AxisBinding ab; + ab.set = true; + ab.index = (uint8_t)b; + ab.lo = lo; + ab.hi = hi; + ab.centre = idleSnapshot[b]; + // A centre outside the swept range means the stick was not resting when + // the window opened; the midpoint is the better guess. + if (ab.centre < lo || ab.centre > hi) ab.centre = (uint8_t)(lo + range / 2); + ab.deadband = (uint8_t)(range / 20 + 2); + axisResult = ab; + axisResultFn = learnAx; + axisLearnDone = true; + if (range < 16) { + Serial.println(F(" warning: that byte barely moved — sweep the stick fully during the window")); + } + return; + } + + // Digital. A byte that swept a wide range is an axis the user asked to bind + // as a button; say so rather than producing a binding that half works. + if (!learnExact && range >= 48) { + Serial.printf(" that looks like an analog axis (range %u). Use `learnaxis %s` instead,\r\n", + range, b == 0 ? "throttle" : "throttle|steer"); + Serial.println(F(" or repeat with `learn exact` to match one exact value.")); + return; + } + + Binding nb; + nb.set = true; + nb.index = (uint8_t)b; + const uint8_t diff = (uint8_t)(idleSnapshot[b] ^ lastDiffValue[b]); + if (!learnExact && diff != 0 && (diff & (diff - 1)) == 0) { + nb.mask = diff; // one bit moved: a combinable button + nb.value = (uint8_t)(lastDiffValue[b] & diff); + } else { + nb.mask = 0xFF; // a value among several: match it exactly + nb.value = lastDiffValue[b]; + } + learnResult = nb; + learnDone = true; +} + +bool learning() { return learnKind != LearnKind::None; } +Fn learnTarget() { return learnFn; } + +bool takeLearned(Fn& f, Binding& b) { + bool got = false; + portENTER_CRITICAL(&mux); + if (learnDone) { f = learnFn; b = learnResult; learnDone = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +bool takeLearnedAxis(Axis& a, AxisBinding& b) { + bool got = false; + portENTER_CRITICAL(&mux); + if (axisLearnDone) { a = axisResultFn; b = axisResult; axisLearnDone = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +float axisValue(Axis a, bool stale) { + const AxisBinding b = axes[a]; + if (!b.set || stale) return 0.0f; + + uint8_t raw = 0; + bool have = false; + portENTER_CRITICAL(&mux); + if (b.index < reportLen) { raw = report[b.index]; have = true; } + portEXIT_CRITICAL(&mux); + if (!have) return 0.0f; + + const int delta = (int)raw - (int)b.centre; + if (abs(delta) <= (int)b.deadband) return 0.0f; + + // Scale each side against its own travel: a stick whose centre is not the + // midpoint still reaches +/-1 at both extremes. + const int span = (delta > 0) ? ((int)b.hi - (int)b.centre) : ((int)b.centre - (int)b.lo); + if (span <= (int)b.deadband) return 0.0f; + + float v = (float)(abs(delta) - b.deadband) / (float)(span - b.deadband); + if (v > 1.0f) v = 1.0f; + if (delta < 0) v = -v; + return b.invert ? -v : v; +} + +const AxisBinding& axis(Axis a) { return axes[a]; } +void setAxis(Axis a, const AxisBinding& b) { axes[a] = b; } +void clearAxes() { for (int i = 0; i < AX_COUNT; i++) axes[i] = AxisBinding{}; } + +void injectReport(const uint8_t* data, size_t len) { handleReport(data, len); } + +const Binding& binding(Fn f) { return bindings[f]; } +void setBinding(Fn f, const Binding& b) { bindings[f] = b; } +void clearBindings() { for (int i = 0; i < FN_COUNT; i++) bindings[i] = Binding{}; } + +bool takeConnectEvent(bool& nowConnected) { + bool got = false; + portENTER_CRITICAL(&mux); + if (connectEventPending) { nowConnected = connectEventState; connectEventPending = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +} // namespace blepad diff --git a/esp32-ble/src/hoverboard.cpp b/esp32-ble/src/hoverboard.cpp new file mode 100644 index 0000000..be94d28 --- /dev/null +++ b/esp32-ble/src/hoverboard.cpp @@ -0,0 +1,114 @@ +#include "hoverboard.h" + +namespace { + +// XOR of every 16-bit field except the checksum itself, matching +// usart_process_command() and the feedback packer in the firmware. +uint16_t checksumOf(const HoverCommand& c) { + return (uint16_t)(c.start ^ (uint16_t)c.left ^ (uint16_t)c.right ^ c.buzzer); +} + +uint16_t checksumOf(const HoverFeedback& f) { + return (uint16_t)(f.start ^ (uint16_t)f.cmd1 ^ (uint16_t)f.cmd2 ^ + (uint16_t)f.speedR_meas ^ (uint16_t)f.speedL_meas ^ + (uint16_t)f.wheelR_cnt ^ (uint16_t)f.wheelL_cnt ^ + (uint16_t)f.batVoltage ^ (uint16_t)f.boardTemp ^ f.cmdLed); +} + +int16_t clampCmd(int32_t v) { + if (v > CMD_MAX) return CMD_MAX; + if (v < -CMD_MAX) return -CMD_MAX; + return (int16_t)v; +} + +} // namespace + +void HoverboardLink::begin(HardwareSerial& port) { + port_ = &port; + port_->begin(HOVER_BAUD, SERIAL_8N1, PIN_HOVER_RX, PIN_HOVER_TX); +} + +void HoverboardLink::send(int16_t left, int16_t right) { + if (!port_) return; + + // Expire a finished beep here rather than on a timer, so the field is always + // consistent with the frame actually going out. + if (buzzer_ != 0 && (int32_t)(millis() - beepUntilMs_) >= 0) buzzer_ = 0; + + HoverCommand c; + c.start = HOVER_START_FRAME; + c.left = clampCmd(INVERT_LEFT ? -left : left); + c.right = clampCmd(INVERT_RIGHT ? -right : right); + c.buzzer = buzzer_; + c.checksum = checksumOf(c); + + port_->write((const uint8_t*)&c, sizeof(c)); +} + +void HoverboardLink::dropByte() { + // Resync: discard the oldest byte and let the next poll re-test the window. + if (have_ > 0) { + memmove(buf_, buf_ + 1, have_ - 1); + have_--; + } +} + +bool HoverboardLink::poll() { + if (!port_) return false; + bool got = false; + + while (port_->available() > 0) { + if (have_ < sizeof(buf_)) { + buf_[have_++] = (uint8_t)port_->read(); + rxBytes_++; + } + + // Keep the window anchored on a start frame before judging the checksum. + while (have_ >= 2) { + uint16_t start; + memcpy(&start, buf_, sizeof(start)); + if (start == HOVER_START_FRAME) break; + dropByte(); + } + + if (have_ == sizeof(buf_)) { + HoverFeedback candidate; + memcpy(&candidate, buf_, sizeof(candidate)); + + if (candidate.checksum == checksumOf(candidate)) { + fb_ = candidate; + lastFrameMs_ = millis(); + framesOk_++; + have_ = 0; + got = true; + } else { + // A corrupt or misaligned frame: step one byte and hunt again rather + // than throwing away bytes that may hold the real start marker. + framesBad_++; + dropByte(); + } + } + } + + return got; +} + +void HoverboardLink::beep(uint8_t pitch, uint8_t pattern, uint16_t ms) { + buzzer_ = (uint16_t)pitch | ((uint16_t)pattern << 8); + beepUntilMs_ = millis() + ms; +} + +void HoverboardLink::beepOff() { + buzzer_ = 0; +} + +bool HoverboardLink::linkUp() const { + return framesOk_ > 0 && (millis() - lastFrameMs_) < FEEDBACK_TIMEOUT_MS; +} + +int16_t hoverTickDelta(int16_t prev, int16_t curr) { + int32_t d = (int32_t)curr - (int32_t)prev; + d = ((d % ODOM_MODULO) + ODOM_MODULO) % ODOM_MODULO; // -> [0, ODOM_MODULO) + if (d >= ODOM_MODULO / 2) d -= ODOM_MODULO; // -> shortest signed path + return (int16_t)d; +} diff --git a/esp32-ble/src/main.cpp b/esp32-ble/src/main.cpp new file mode 100644 index 0000000..38d8171 --- /dev/null +++ b/esp32-ble/src/main.cpp @@ -0,0 +1,728 @@ +// TranspOtter — BLE gamepad build. +// +// A Bluetooth LE HID controller drives the machine; a USB serial terminal is +// used to scan for it, pair it, and teach it which buttons do what. The motion +// planner, odometry and hoverboard link are shared with the web UI build. +// +// Type `help` in the monitor for the command list. + +#include +#include + +#include "robot_config.h" +#include "hoverboard.h" +#include "motion.h" +#include "ble_pad.h" + +namespace { + +HoverboardLink hoverLink; +Odometry odo; +Motion motion; +Preferences prefs; + +uint32_t bootMs = 0, lastCmdMs = 0; +bool armed = false, linkWasUp = false, telemetry = false; +uint32_t lastTelemMs = 0; + +float speedLimit = SPEED_LIMIT_DEFAULT; +float savedResponse = RESPONSE_DEFAULT; +bool turbo = false, turboPrev = false; +bool brakeReleasedPrev = false; +bool rawDump = false; + +// How the direction buttons behave. Brake has its own setting below, because +// a toggle and a hold-to-release are different ideas and a given remote may +// want one for steering and the other for the brake. +enum class InputMode : uint8_t { Hold, Latch, Pulse, Analog }; +InputMode inputMode = InputMode::Hold; // directions report release, so level works +uint16_t pulseMs = PULSE_MS_DEFAULT; +uint32_t idleStopMs = IDLE_STOP_MS_DEFAULT; +bool brakeRequired = true; + +enum class BrakeMode : uint8_t { Toggle, Hold }; +BrakeMode brakeMode = BrakeMode::Toggle; // click on, click off +uint32_t staleMs = INPUT_STALE_MS_DEFAULT; + +// Edge detection state: a repeated report while a button is held must read as +// one press, not as one press per report. +bool wasActive[blepad::FN_COUNT] = {false}; +uint32_t lastReportMs = 0; + +// Latched intent, in latch and pulse modes. +int8_t latchDrive = 0; // +1 forward, -1 back +int8_t latchTurn = 0; // +1 left, -1 right +float analogThrottle = 0, analogSteer = 0; +bool brakeEngaged = true; // start braked, always +uint32_t pulseDriveUntil = 0, pulseTurnUntil = 0; +uint32_t lastEventMs = 0; + +void clearLatches(); // defined with the gamepad service below + +const char* modeName(InputMode m) { + switch (m) { + case InputMode::Hold: return "hold"; + case InputMode::Latch: return "latch"; + case InputMode::Pulse: return "pulse"; + default: return "analog"; + } +} + +const char* brakeModeName(BrakeMode m) { return m == BrakeMode::Toggle ? "toggle" : "hold"; } + +String lineBuf; +SemaphoreHandle_t controlMux = nullptr; + +// --------------------------------------------------------- persistence --- + +void saveAll() { + prefs.putFloat("limit", speedLimit); + prefs.putFloat("response", savedResponse); + prefs.putFloat("decel", motion.decelBoost()); + uint8_t blob[blepad::FN_COUNT * 4]; + for (int i = 0; i < blepad::FN_COUNT; i++) { + const blepad::Binding& b = blepad::binding((blepad::Fn)i); + blob[i * 4 + 0] = b.set ? 1 : 0; + blob[i * 4 + 1] = b.index; + blob[i * 4 + 2] = b.mask; + blob[i * 4 + 3] = b.value; + } + prefs.putBytes("map", blob, sizeof(blob)); + + uint8_t ab[blepad::AX_COUNT * 7]; + for (int i = 0; i < blepad::AX_COUNT; i++) { + const blepad::AxisBinding& a = blepad::axis((blepad::Axis)i); + ab[i*7+0] = a.set ? 1 : 0; ab[i*7+1] = a.index; ab[i*7+2] = a.centre; + ab[i*7+3] = a.lo; ab[i*7+4] = a.hi; ab[i*7+5] = a.deadband; + ab[i*7+6] = a.invert ? 1 : 0; + } + prefs.putBytes("axes", ab, sizeof(ab)); + prefs.putUChar("inmode", (uint8_t)inputMode); + prefs.putUShort("pulse", pulseMs); + prefs.putULong("idle", idleStopMs); + prefs.putBool("brakereq", brakeRequired); + prefs.putUChar("brakemode", (uint8_t)brakeMode); + prefs.putULong("stale", staleMs); + Serial.println(F("saved")); +} + +void loadAll() { + speedLimit = prefs.getFloat("limit", SPEED_LIMIT_DEFAULT); + savedResponse = prefs.getFloat("response", RESPONSE_DEFAULT); + motion.setDecelBoost(prefs.getFloat("decel", DECEL_BOOST_DEFAULT)); + motion.setResponse(savedResponse); + + inputMode = (InputMode)prefs.getUChar("inmode", (uint8_t)InputMode::Latch); + pulseMs = prefs.getUShort("pulse", PULSE_MS_DEFAULT); + idleStopMs = prefs.getULong("idle", IDLE_STOP_MS_DEFAULT); + brakeRequired = prefs.getBool("brakereq", true); + brakeMode = (BrakeMode)prefs.getUChar("brakemode", (uint8_t)BrakeMode::Toggle); + staleMs = prefs.getULong("stale", INPUT_STALE_MS_DEFAULT); + + uint8_t blob[blepad::FN_COUNT * 4] = {0}; + if (prefs.getBytes("map", blob, sizeof(blob)) == sizeof(blob)) { + for (int i = 0; i < blepad::FN_COUNT; i++) { + blepad::Binding b; + b.set = blob[i * 4 + 0] != 0; + b.index = blob[i * 4 + 1]; + b.mask = blob[i * 4 + 2]; + b.value = blob[i * 4 + 3]; + blepad::setBinding((blepad::Fn)i, b); + } + } + + uint8_t ab[blepad::AX_COUNT * 7] = {0}; + if (prefs.getBytes("axes", ab, sizeof(ab)) == sizeof(ab)) { + for (int i = 0; i < blepad::AX_COUNT; i++) { + blepad::AxisBinding a; + a.set = ab[i*7+0] != 0; a.index = ab[i*7+1]; a.centre = ab[i*7+2]; + a.lo = ab[i*7+3]; a.hi = ab[i*7+4]; a.deadband = ab[i*7+5]; + a.invert = ab[i*7+6] != 0; + blepad::setAxis((blepad::Axis)i, a); + } + } +} + +// --------------------------------------------------------------- output --- + +void printHelp() { + Serial.println(); + Serial.println(F("TranspOtter — BLE gamepad build")); + Serial.println(F(" pairing:")); + Serial.println(F(" scan [secs] list BLE devices (HID ones are marked)")); + Serial.println(F(" connect pair and subscribe to its reports")); + Serial.println(F(" disconnect / forget drop the link and stop retrying / erase all bonds")); + Serial.println(F(" autoreconnect on|off keep retrying the bonded controller (default on)")); + Serial.println(F(" mapping:")); + Serial.println(F(" raw on|off dump HID reports as hex")); + Serial.println(F(" learn [exact] then press the button. fn = fwd back left right brake turbo stop")); + Serial.println(F(" 'exact' matches the whole byte, for remotes that send key codes")); + Serial.println(F(" bind set a binding by hand (0x.. accepted)")); + Serial.println(F(" map / clearmap / save show, wipe, persist")); + Serial.println(F(" input model (for remotes that never report button release):")); + Serial.println(F(" inputmode hold|latch|pulse|analog how the DIRECTIONS are read")); + Serial.println(F(" learnaxis throttle|steer [secs] sweep the stick fully both ways")); + Serial.println(F(" axis show axis bindings")); + Serial.println(F(" bindaxis [deadband] [invert]")); + Serial.println(F(" clearaxes wipe axis bindings")); + Serial.println(F(" inject feed a synthetic report, for testing a mapping")); + Serial.println(F(" brakemode toggle|hold click on/off, or hold-to-release")); + Serial.println(F(" stale buttons count as released after this long with no report")); + Serial.println(F(" pulse drive time per click in pulse mode")); + Serial.println(F(" idle clear latches after this long with no button (0 = never)")); + Serial.println(F(" brake require|ignore whether the brake interlock is enforced")); + Serial.println(F(" driving:")); + Serial.println(F(" speed <0..1> normal throttle scale")); + Serial.println(F(" resp <0..1> acceleration sharpness")); + Serial.println(F(" decel <1..6> how much firmer braking is than acceleration")); + Serial.println(F(" stop / estop / arm / zero")); + Serial.println(F(" beep [pattern] [ms]")); + Serial.println(F(" status / tel [on|off]")); + Serial.println(); + Serial.println(F(" Presses are detected on the rising edge, so a controller that repeats a")); + Serial.println(F(" report 10-20x/s while held counts as one press, not twenty.")); + Serial.println(F(" directions: hold = drive while held; latch = click on/off; pulse = timed.")); + Serial.println(F(" brake/turbo/stop are always edge-triggered.")); + Serial.println(); +} + +void printMap() { + Serial.println(F("function byte mask value")); + for (int i = 0; i < blepad::FN_COUNT; i++) { + const blepad::Binding& b = blepad::binding((blepad::Fn)i); + if (b.set) { + Serial.printf(" %-7s %4u 0x%02X 0x%02X %s\r\n", blepad::FN_NAME[i], + b.index, b.mask, b.value, + blepad::bindingActive((blepad::Fn)i) ? "<-- active now" : ""); + } else { + Serial.printf(" %-7s (unmapped)\r\n", blepad::FN_NAME[i]); + } + } +} + +void printStatus() { + Serial.printf("mode=%-8s armed=%d link=%d pad=%d turbo=%d cmd=%+5d/%+5d v=%+.3f w=%+.3f\r\n", + motion.modeName(), armed ? 1 : 0, hoverLink.linkUp() ? 1 : 0, + blepad::connected() ? 1 : 0, turbo ? 1 : 0, + motion.cmdLeft(), motion.cmdRight(), motion.linear(), motion.angular()); + Serial.printf("odo: dist=%+.3f m head=%+.1f deg\r\n", odo.distance(), odo.heading() * 180.0 / PI_F); + Serial.printf("tuning: speed %.0f%% response %.2f decel %.2fx (turbo: %.0f%% / %.2f)\r\n", + speedLimit * 100.0f, motion.response(), motion.decelBoost(), + TURBO_SPEED * 100.0f, TURBO_RESPONSE); + Serial.printf("input: dirs=%s brake=%s/%s pulse %u ms idle %.1f s stale %lu ms\r\n", + modeName(inputMode), brakeModeName(brakeMode), + brakeRequired ? "require" : "ignore", + pulseMs, idleStopMs / 1000.0f, (unsigned long)staleMs); + if (inputMode == InputMode::Analog) { + Serial.printf("state: throttle=%+.2f steer=%+.2f brake=%s\r\n", + analogThrottle, analogSteer, brakeEngaged ? "ENGAGED" : "released"); + } else { + Serial.printf("state: drive=%+d turn=%+d brake=%s\r\n", + latchDrive, latchTurn, brakeEngaged ? "ENGAGED" : "released"); + } + Serial.printf("pad: %s target %s autoreconnect %s%s\r\n", + blepad::connected() ? "connected" : "not connected", + blepad::targetString(), blepad::autoReconnect() ? "on" : "off", + blepad::busy() ? " (worker busy)" : ""); + Serial.printf("board: %.2f V %.1f C frames ok=%lu bad=%lu reports=%lu\r\n", + hoverLink.batteryVolts(), hoverLink.boardTempC(), + (unsigned long)hoverLink.framesOk(), (unsigned long)hoverLink.framesBad(), + (unsigned long)blepad::reportCount()); +} + +// --------------------------------------------------------------- console --- + +bool applyWord(const char* w) { + if (!strcmp(w, "stop")) { motion.stop(); Serial.println(F("stopping")); } + else if (!strcmp(w, "estop")) { motion.emergencyStop(); Serial.println(F("E-STOP latched — send `arm` to clear")); } + else if (!strcmp(w, "arm")) { motion.clearEStop(); Serial.println(F("cleared")); } + else if (!strcmp(w, "zero")) { odo.reset(); Serial.println(F("odometry zeroed")); } + else return false; + return true; +} + +void handleLine(String line) { + line.trim(); + if (line.isEmpty()) return; + + char verb[16] = {0}, word[24] = {0}, word2[16] = {0}; + float a1 = 0, a2 = 0, a3 = 0; + const int n = sscanf(line.c_str(), "%15s %f %f %f", verb, &a1, &a2, &a3); + const int nWord = sscanf(line.c_str(), "%15s %23s %15s", verb, word, word2); + if (n < 1 && nWord < 1) return; + const String cmd(verb); + + if (cmd == "help" || cmd == "?") { + printHelp(); + } else if (cmd == "status" || cmd == "st") { + printStatus(); + } else if (cmd == "scan") { + blepad::requestScan(n >= 2 ? (uint32_t)a1 : 6); + Serial.println(F("scan queued; results will print here")); + if (blepad::busy()) Serial.println(F(" (a reconnect attempt is finishing first)")); + } else if (cmd == "connect") { + if (nWord < 2) { Serial.println(F("usage: connect ")); return; } + const bool byIndex = (word[0] >= '0' && word[0] <= '9' && strchr(word, ':') == nullptr); + const bool ok = byIndex ? blepad::requestConnectIndex((size_t)atoi(word)) + : blepad::requestConnectAddress(word); + Serial.println(ok ? F("target set — connecting in the background, and retrying until it succeeds") + : F("no such device in the scan list")); + } else if (cmd == "autoreconnect") { + if (nWord < 2) { Serial.printf("autoreconnect %s\r\n", blepad::autoReconnect() ? "on" : "off"); return; } + blepad::setAutoReconnect(!strcmp(word, "on") || !strcmp(word, "1")); + Serial.printf("autoreconnect %s\r\n", blepad::autoReconnect() ? "on" : "off"); + } else if (cmd == "disconnect") { + blepad::disconnect(); + Serial.println(F("disconnected and target cleared — `connect` again to re-pair")); + } else if (cmd == "forget") { + blepad::forgetBonds(); + Serial.println(F("bonds erased")); + } else if (cmd == "raw") { + rawDump = (nWord >= 2) ? (!strcmp(word, "on") || !strcmp(word, "1")) : !rawDump; + Serial.printf("raw dump %s\r\n", rawDump ? "on" : "off"); + } else if (cmd == "learn") { + if (nWord < 2) { Serial.println(F("usage: learn ")); return; } + int f = -1; + for (int i = 0; i < blepad::FN_COUNT; i++) if (!strcmp(word, blepad::FN_NAME[i])) f = i; + if (f < 0) { Serial.println(F("unknown function")); return; } + // Reports are what learning needs, not a connection — `inject` can supply + // them, which is how a mapping gets tested without the controller present. + if (blepad::reportCount() == 0) { + Serial.println(F("no reports yet — connect a pad, or use `inject` to test")); + return; + } + const bool exact = (nWord >= 3) && !strcmp(word2, "exact"); + blepad::startLearn((blepad::Fn)f, exact); + Serial.printf("learning `%s`%s — press that button now\r\n", + blepad::FN_NAME[f], exact ? " (exact byte match)" : ""); + } else if (cmd == "learnaxis") { + if (nWord < 2) { Serial.println(F("usage: learnaxis throttle|steer [seconds]")); return; } + int a = -1; + for (int i = 0; i < blepad::AX_COUNT; i++) if (!strcmp(word, blepad::AX_NAME[i])) a = i; + if (a < 0) { Serial.println(F("unknown axis")); return; } + const uint32_t ms = (n >= 3) ? (uint32_t)(a2 * 1000.0f) : 4000; + blepad::startAxisLearn((blepad::Axis)a, ms); + Serial.printf("learning axis `%s` for %.1f s — leave it centred, then sweep FULLY both ways\r\n", + blepad::AX_NAME[a], ms / 1000.0f); + } else if (cmd == "axis") { + for (int i = 0; i < blepad::AX_COUNT; i++) { + const blepad::AxisBinding& a = blepad::axis((blepad::Axis)i); + if (a.set) { + Serial.printf(" %-8s byte %u centre 0x%02X range 0x%02X..0x%02X deadband %u%s -> %+.2f\r\n", + blepad::AX_NAME[i], a.index, a.centre, a.lo, a.hi, a.deadband, + a.invert ? " inverted" : "", + blepad::axisValue((blepad::Axis)i, false)); + } else { + Serial.printf(" %-8s (unmapped)\r\n", blepad::AX_NAME[i]); + } + } + } else if (cmd == "bindaxis") { + char an[16] = {0}; int bi=0, ce=0, lo=0, hi=0, db=6, iv=0; + const int got = sscanf(line.c_str(), "%*s %15s %i %i %i %i %i %i", an, &bi, &ce, &lo, &hi, &db, &iv); + if (got < 5) { Serial.println(F("usage: bindaxis [deadband] [invert]")); return; } + int a = -1; + for (int i = 0; i < blepad::AX_COUNT; i++) if (!strcmp(an, blepad::AX_NAME[i])) a = i; + if (a < 0) { Serial.println(F("unknown axis")); return; } + blepad::AxisBinding ab; + ab.set = true; ab.index = (uint8_t)bi; ab.centre = (uint8_t)ce; + ab.lo = (uint8_t)lo; ab.hi = (uint8_t)hi; + ab.deadband = (uint8_t)((got >= 6) ? db : 6); + ab.invert = (got >= 7) && iv != 0; + blepad::setAxis((blepad::Axis)a, ab); + Serial.printf("axis `%s` bound (`save` to keep)\r\n", blepad::AX_NAME[a]); + } else if (cmd == "clearaxes") { + blepad::clearAxes(); + Serial.println(F("axis bindings cleared")); + } else if (cmd == "inject") { + uint8_t buf[32]; size_t len = 0; + const char* q = line.c_str(); + while (*q && *q != ' ') q++; // skip the verb + while (*q && len < sizeof(buf)) { + while (*q == ' ') q++; + if (!*q) break; + buf[len++] = (uint8_t)strtol(q, (char**)&q, 16); + } + if (len == 0) { Serial.println(F("usage: inject AA BB CC ...")); return; } + blepad::injectReport(buf, len); + Serial.printf("injected %u byte(s)\r\n", (unsigned)len); + } else if (cmd == "map") { + printMap(); + } else if (cmd == "clearmap") { + blepad::clearBindings(); + Serial.println(F("mapping cleared")); + } else if (cmd == "save") { + saveAll(); + } else if (cmd == "beep") { + const uint8_t pitch = (n >= 2) ? (uint8_t)a1 : 8; + const uint8_t pattern = (n >= 3) ? (uint8_t)a2 : 0; + const uint16_t ms = (n >= 4) ? (uint16_t)a3 : 200; + hoverLink.beep(pitch, pattern, ms); + Serial.printf("beep pitch %u pattern %u for %u ms\r\n", pitch, pattern, ms); + } else if (cmd == "speed") { + if (n < 2) { Serial.printf("speed %.0f%%\r\n", speedLimit * 100.0f); return; } + speedLimit = constrain(a1, 0.05f, 1.0f); + Serial.printf("speed %.0f%% (`save` to keep)\r\n", speedLimit * 100.0f); + } else if (cmd == "resp") { + if (n < 2) { Serial.printf("response %.2f\r\n", motion.response()); return; } + savedResponse = constrain(a1, 0.0f, 1.0f); + if (!turbo) motion.setResponse(savedResponse); + Serial.printf("response %.2f (`save` to keep)\r\n", savedResponse); + } else if (cmd == "bind") { + char fname[16] = {0}; + int bi = 0, bm = 0, bv = 0; + if (sscanf(line.c_str(), "%*s %15s %i %i %i", fname, &bi, &bm, &bv) != 4) { + Serial.println(F("usage: bind e.g. bind fwd 2 0xFF 0x05")); + return; + } + int f = -1; + for (int i = 0; i < blepad::FN_COUNT; i++) if (!strcmp(fname, blepad::FN_NAME[i])) f = i; + if (f < 0) { Serial.println(F("unknown function")); return; } + blepad::Binding b; + b.set = true; b.index = (uint8_t)bi; b.mask = (uint8_t)bm; b.value = (uint8_t)bv; + blepad::setBinding((blepad::Fn)f, b); + Serial.printf("bound `%s` to byte %u mask 0x%02X value 0x%02X (`save` to keep)\r\n", + blepad::FN_NAME[f], b.index, b.mask, b.value); + } else if (cmd == "inputmode") { + if (nWord < 2) { Serial.printf("inputmode %s\r\n", modeName(inputMode)); return; } + if (!strcmp(word, "hold")) inputMode = InputMode::Hold; + else if (!strcmp(word, "latch")) inputMode = InputMode::Latch; + else if (!strcmp(word, "pulse")) inputMode = InputMode::Pulse; + else if (!strcmp(word, "analog")) inputMode = InputMode::Analog; + else { Serial.println(F("usage: inputmode hold|latch|pulse|analog")); return; } + clearLatches(); + brakeEngaged = true; + Serial.printf("inputmode %s (`save` to keep)\r\n", modeName(inputMode)); + if (inputMode == InputMode::Hold) { + Serial.println(F(" note: hold mode needs a controller that reports button RELEASE.")); + } + } else if (cmd == "pulse") { + if (n < 2) { Serial.printf("pulse %u ms\r\n", pulseMs); return; } + pulseMs = (uint16_t)constrain(a1, 100.0f, 10000.0f); + Serial.printf("pulse %u ms (`save` to keep)\r\n", pulseMs); + } else if (cmd == "idle") { + if (n < 2) { Serial.printf("idle %.1f s\r\n", idleStopMs / 1000.0f); return; } + idleStopMs = (uint32_t)(constrain(a1, 0.0f, 600.0f) * 1000.0f); + if (idleStopMs == 0) { + Serial.println(F("idle timeout DISABLED — a lost remote will leave it driving.")); + } else { + Serial.printf("idle %.1f s (`save` to keep)\r\n", idleStopMs / 1000.0f); + } + } else if (cmd == "brakemode") { + if (nWord < 2) { Serial.printf("brakemode %s\r\n", brakeModeName(brakeMode)); return; } + if (!strcmp(word, "toggle")) brakeMode = BrakeMode::Toggle; + else if (!strcmp(word, "hold")) brakeMode = BrakeMode::Hold; + else { Serial.println(F("usage: brakemode toggle|hold")); return; } + brakeEngaged = true; + clearLatches(); + Serial.printf("brakemode %s (`save` to keep)\r\n", brakeModeName(brakeMode)); + } else if (cmd == "stale") { + if (n < 2) { Serial.printf("stale %lu ms\r\n", (unsigned long)staleMs); return; } + staleMs = (uint32_t)constrain(a1, 50.0f, 5000.0f); + Serial.printf("stale %lu ms (`save` to keep)\r\n", (unsigned long)staleMs); + } else if (cmd == "brake") { + if (nWord < 2) { Serial.printf("brake %s\r\n", brakeRequired ? "require" : "ignore"); return; } + if (!strcmp(word, "require")) { brakeRequired = true; Serial.println(F("brake interlock required")); } + else if (!strcmp(word, "ignore")) { brakeRequired = false; Serial.println(F("brake interlock IGNORED — nothing will stop it but `stop` or the idle timeout")); } + else { Serial.println(F("usage: brake require|ignore")); return; } + } else if (applyWord(verb)) { + // stop / estop / arm / zero + } else if (cmd == "decel") { + if (n < 2) { Serial.printf("decel %.2fx\r\n", motion.decelBoost()); return; } + motion.setDecelBoost(a1); + Serial.printf("decel %.2fx — brake onset lag now %.0f ms (`save` to keep)\r\n", + motion.decelBoost(), + 1000.0f * (A_MAX_MPS2 + (A_MAX_SHARP - A_MAX_MPS2) * motion.response()) / + ((J_MAX_MPS3 + (J_MAX_SHARP - J_MAX_MPS3) * motion.response()) + * motion.decelBoost())); + } else if (cmd == "tel") { + telemetry = (nWord < 2) ? !telemetry : (!strcmp(word, "on") || !strcmp(word, "1")); + Serial.printf("telemetry %s\r\n", telemetry ? "on" : "off"); + } else { + Serial.printf("unknown command: %s (try `help`)\r\n", verb); + } +} + +void pumpConsole() { + while (Serial.available() > 0) { + const char c = (char)Serial.read(); + if (c == '\n' || c == '\r') { + if (lineBuf.length() > 0) { handleLine(lineBuf); lineBuf = ""; } + } else if (lineBuf.length() < 96) { + lineBuf += c; + } + } +} + +// ------------------------------------------------------------ gamepad --- + +void clearLatches() { + latchDrive = latchTurn = 0; +} + +void setDriveLatch(int8_t dir, uint32_t now) { + if (inputMode == InputMode::Pulse) { + latchDrive = dir; + pulseDriveUntil = now + pulseMs; + } else { + latchDrive = (latchDrive == dir) ? 0 : dir; // same button again = off + } +} + +void setTurnLatch(int8_t dir, uint32_t now) { + if (inputMode == InputMode::Pulse) { + latchTurn = dir; + pulseTurnUntil = now + pulseMs; + } else { + latchTurn = (latchTurn == dir) ? 0 : dir; + } +} + +void setTurbo(bool on) { + if (turbo == on) return; + turbo = on; + motion.setResponse(turbo ? TURBO_RESPONSE : savedResponse); + hoverLink.beep(turbo ? BEEP_TURBO_ON : BEEP_TURBO_OFF, 0, + turbo ? BEEP_TURBO_ON_MS : BEEP_TURBO_OFF_MS); + Serial.printf("turbo %s\r\n", turbo ? "ON" : "off"); +} + +// A direction click, in latch or pulse mode. +void applyDirectionClick(blepad::Fn f, uint32_t now) { + switch (f) { + case blepad::FN_FWD: setDriveLatch(+1, now); break; + case blepad::FN_BACK: setDriveLatch(-1, now); break; + case blepad::FN_LEFT: setTurnLatch(+1, now); break; + case blepad::FN_RIGHT: setTurnLatch(-1, now); break; + default: break; + } +} + +void serviceGamepad(uint32_t now) { + bool nowConnected; + if (blepad::takeConnectEvent(nowConnected)) { + if (nowConnected) { + Serial.println(F("pad connected")); + hoverLink.beep(BEEP_CONNECT, 0, BEEP_CONNECT_MS); + } else { + Serial.println(F("! pad disconnected — stopping")); + hoverLink.beep(BEEP_DISCONNECT, 0, BEEP_DISCONNECT_MS); + motion.stop(); + clearLatches(); + brakeEngaged = true; // never come back from a dropout rolling + setTurbo(false); + } + } + + blepad::Axis la; blepad::AxisBinding lab; + if (blepad::takeLearnedAxis(la, lab)) { + blepad::setAxis(la, lab); + Serial.printf("learned axis `%s`: byte %u centre 0x%02X range 0x%02X..0x%02X deadband %u (`save` to keep)\r\n", + blepad::AX_NAME[la], lab.index, lab.centre, lab.lo, lab.hi, lab.deadband); + hoverLink.beep(BEEP_BRAKE_OFF, 0, 80); + } + + blepad::Fn lf; blepad::Binding lb; + if (blepad::takeLearned(lf, lb)) { + blepad::setBinding(lf, lb); + Serial.printf("learned `%s`: byte %u mask 0x%02X value 0x%02X (`save` to keep)\r\n", + blepad::FN_NAME[lf], lb.index, lb.mask, lb.value); + hoverLink.beep(BEEP_BRAKE_OFF, 0, 80); + } + + uint8_t rep[32]; + size_t rlen = 0; + if (blepad::takeReportEvent(rep, sizeof(rep), rlen)) { + lastReportMs = now; + if (rawDump) { + Serial.print("report:"); + for (size_t i = 0; i < rlen; i++) Serial.printf(" %02X", rep[i]); + Serial.println(); + } + } + + if (!blepad::connected()) return; + + const bool brakeMapped = blepad::binding(blepad::FN_BRAKE).set; + if (brakeRequired && !brakeMapped) { + static uint32_t nagMs = 0; + if (now - nagMs > 5000) { + nagMs = now; + Serial.println(F("brake unmapped — driving disabled. `learn brake`, or `brake ignore`")); + } + motion.setDrive(0, 0); + lastCmdMs = now; + return; + } + + // A controller that repeats while held keeps this fresh; one that went quiet + // is treated as all-buttons-up, so a dropped release report cannot leave a + // direction stuck on. + const bool stale = (now - lastReportMs) > staleMs; + + bool active[blepad::FN_COUNT], rising[blepad::FN_COUNT]; + for (int i = 0; i < blepad::FN_COUNT; i++) { + active[i] = stale ? false : blepad::bindingActive((blepad::Fn)i); + rising[i] = active[i] && !wasActive[i]; + } + + // ---- brake ------------------------------------------------------------- + if (brakeMode == BrakeMode::Toggle) { + if (rising[blepad::FN_BRAKE]) { + brakeEngaged = !brakeEngaged; + if (brakeEngaged) clearLatches(); + hoverLink.beep(brakeEngaged ? BEEP_BRAKE_ON : BEEP_BRAKE_OFF, 0, + brakeEngaged ? BEEP_BRAKE_ON_MS : BEEP_BRAKE_OFF_MS); + Serial.printf("brake %s\r\n", brakeEngaged ? "ENGAGED" : "released"); + } + } else { // hold-to-release + const bool released = brakeMapped ? active[blepad::FN_BRAKE] : true; + if (released == brakeEngaged) { // state changed + hoverLink.beep(released ? BEEP_BRAKE_OFF : BEEP_BRAKE_ON, 0, + released ? BEEP_BRAKE_OFF_MS : BEEP_BRAKE_ON_MS); + Serial.printf("brake %s\r\n", released ? "released" : "ENGAGED"); + } + brakeEngaged = !released; + if (brakeEngaged) clearLatches(); + } + + // ---- stop and turbo ---------------------------------------------------- + if (rising[blepad::FN_STOP]) { + clearLatches(); + brakeEngaged = true; + setTurbo(false); + hoverLink.beep(BEEP_BRAKE_ON, 0, 250); + Serial.println(F("STOP — latches cleared, brake engaged")); + } + if (rising[blepad::FN_TURBO] && !brakeEngaged) setTurbo(!turbo); + + // ---- directions -------------------------------------------------------- + if (inputMode == InputMode::Analog) { + analogThrottle = blepad::axisValue(blepad::AX_THROTTLE, stale); + analogSteer = blepad::axisValue(blepad::AX_STEER, stale); + } else if (inputMode == InputMode::Hold) { + latchDrive = (int8_t)((active[blepad::FN_FWD] ? 1 : 0) - (active[blepad::FN_BACK] ? 1 : 0)); + latchTurn = (int8_t)((active[blepad::FN_LEFT] ? 1 : 0) - (active[blepad::FN_RIGHT] ? 1 : 0)); + } else { + for (int i = blepad::FN_FWD; i <= blepad::FN_RIGHT; i++) { + if (rising[i]) applyDirectionClick((blepad::Fn)i, now); + } + if (inputMode == InputMode::Pulse) { + if (latchDrive != 0 && (int32_t)(now - pulseDriveUntil) >= 0) latchDrive = 0; + if (latchTurn != 0 && (int32_t)(now - pulseTurnUntil) >= 0) latchTurn = 0; + } else if (idleStopMs != 0 && (latchDrive != 0 || latchTurn != 0) && + (now - lastReportMs) > idleStopMs) { + clearLatches(); + hoverLink.beep(BEEP_DISCONNECT, 0, 400); + Serial.println(F("! idle timeout — latches cleared")); + } + } + + for (int i = 0; i < blepad::FN_COUNT; i++) wasActive[i] = active[i]; + + const bool blocked = brakeRequired && brakeEngaged; + const float rawThrottle = (inputMode == InputMode::Analog) ? analogThrottle : (float)latchDrive; + const float rawSteer = (inputMode == InputMode::Analog) ? analogSteer : (float)latchTurn; + float throttle = blocked ? 0.0f : rawThrottle; + float steer = blocked ? 0.0f : rawSteer; + throttle *= turbo ? TURBO_SPEED : speedLimit; + + motion.setDrive(throttle, steer); + lastCmdMs = now; +} + +// Everything with a deadline runs here, above the console, the BLE worker and +// the Arduino loop task. The mainboard cuts the motors if a command frame is +// more than 0.8 s late, and a blocked Serial write or a BLE scan must never be +// able to cause that. +void controlTick(uint32_t now, float dt) { + if (hoverLink.poll()) { + const HoverFeedback& fb = hoverLink.feedback(); + odo.update(fb.wheelL_cnt, fb.wheelR_cnt); + } + + if (!armed && (now - bootMs) >= ARM_HOLD_MS) { + armed = true; + Serial.println(F("armed — type `help` for commands")); + } + + const bool up = hoverLink.linkUp(); + if (linkWasUp && !up) { motion.stop(); Serial.println(F("! feedback lost — ramping down")); } + else if (!linkWasUp && up) { Serial.println(F("feedback link up")); } + linkWasUp = up; + + blepad::poll(now); + serviceGamepad(now); + + if ((motion.mode() == Motion::Mode::Drive || motion.mode() == Motion::Mode::Velocity) && + (now - lastCmdMs) > COMMAND_DEADMAN_MS) { + motion.stop(); + Serial.println(F("! command timeout — ramping down")); + } + + motion.update(odo, dt); + if (motion.takeAbort()) Serial.println(F("! move abandoned — no odometry progress")); + + hoverLink.send(armed ? motion.cmdLeft() : 0, armed ? motion.cmdRight() : 0); +} + +void controlTask(void*) { + TickType_t last = xTaskGetTickCount(); + uint32_t prev = millis(); + for (;;) { + vTaskDelayUntil(&last, pdMS_TO_TICKS(CONTROL_PERIOD_MS)); + const uint32_t now = millis(); + const float dt = (now - prev) / 1000.0f; + prev = now; + + if (xSemaphoreTake(controlMux, pdMS_TO_TICKS(50)) == pdTRUE) { + controlTick(now, dt > 0.0f ? dt : 0.001f); + xSemaphoreGive(controlMux); + } + } +} + +} // namespace + +void setup() { + // Bound how long a write may block. The default is 100 ms, so a terminal + // that is slow to drain stalls this task and the console starts losing + // keystrokes as the receive ring overflows. Dropping a log line is always + // preferable to stalling. + Serial.setTxTimeoutMs(10); + Serial.setRxBufferSize(512); + Serial.begin(115200); + + hoverLink.begin(Serial1); + + prefs.begin("transpotter", false); + loadAll(); + blepad::begin(); + + bootMs = millis(); + lastCmdMs = bootMs; + + controlMux = xSemaphoreCreateMutex(); + xTaskCreate(controlTask, "control", 4096, nullptr, 5, nullptr); + + Serial.println(); + Serial.println(F("TranspOtter (BLE gamepad) — holding zero to arm the mainboard")); +} + +void loop() { + // Nothing time-critical lives here any more. The console can block on a slow + // terminal for as long as it likes without the mainboard noticing. + pumpConsole(); + + const uint32_t now = millis(); + if (telemetry && (now - lastTelemMs) >= 200) { + lastTelemMs = now; + if (xSemaphoreTake(controlMux, pdMS_TO_TICKS(20)) == pdTRUE) { + Serial.printf("t=%lu cmd=%+5d/%+5d v=%+.3f w=%+.3f pad=%d brake=%d turbo=%d dist=%+.3f\r\n", + (unsigned long)now, motion.cmdLeft(), motion.cmdRight(), + motion.linear(), motion.angular(), blepad::connected() ? 1 : 0, + brakeEngaged ? 1 : 0, turbo ? 1 : 0, odo.distance()); + xSemaphoreGive(controlMux); + } + } + delay(2); +} diff --git a/esp32-ble/src/motion.cpp b/esp32-ble/src/motion.cpp new file mode 100644 index 0000000..84e174e --- /dev/null +++ b/esp32-ble/src/motion.cpp @@ -0,0 +1,316 @@ +#include "motion.h" +#include "hoverboard.h" // hoverTickDelta +#include + +namespace { + +float clampf(float v, float lo, float hi) { + return v < lo ? lo : (v > hi ? hi : v); +} + +// Fastest approach speed from which the axis can still stop within `remaining`, +// given that deceleration cannot appear instantly but has to be ramped in at +// the jerk limit. +// +// Stopping from v costs v^2/(2a) while decelerating plus v*(a/j)/2 while the +// deceleration ramps up. Setting that equal to the distance left and solving +// for v gives the expression below. A plain sqrt(2*a*d) ignores the second +// term, which is exactly what makes a heavy robot sail past its mark. +float approachVelocity(float remaining, float amax, float jmax, + float vcap, float creep, float tol) { + const float d = fabsf(remaining); + if (d < tol) return 0.0f; + + // Twice the ramp time, not once: when the profile crosses from accelerating + // to braking, the axis is still at +amax and has to swing all the way to + // -amax, which is two jerk ramps rather than one. Budgeting for a single ramp + // is what let a move overshoot its target and then hunt back and forth. + const float tj = 2.0f * amax / jmax; + const float v = 0.5f * (-amax * tj + + sqrtf(amax * amax * tj * tj + 8.0f * amax * d)); + + return copysignf(fmaxf(fminf(v, vcap), creep), remaining); +} + +} // namespace + +// ------------------------------------------------------------- Odometry --- + +void Odometry::reset() { + left_m_ = right_m_ = 0; + started_ = false; +} + +void Odometry::update(int16_t rawLeft, int16_t rawRight) { + if (!started_) { + prevL_ = rawLeft; + prevR_ = rawRight; + started_ = true; + return; + } + + int16_t dL = hoverTickDelta(prevL_, rawLeft); + int16_t dR = hoverTickDelta(prevR_, rawRight); + prevL_ = rawLeft; + prevR_ = rawRight; + + if (INVERT_ODOM_LEFT) dL = -dL; + if (INVERT_ODOM_RIGHT) dR = -dR; + + left_m_ += dL * (double)METRES_PER_TICK; + right_m_ += dR * (double)METRES_PER_TICK; +} + +// ------------------------------------------------------------- JerkAxis --- + +void JerkAxis::update(float target, float dt) { + if (dt <= 0.0f) return; + + target = clampf(target, -vmax_, vmax_); + + // Slowing down, or reversing through zero, gets the firmer limits. + const bool slowing = (target == 0.0f) || (fabsf(target) < fabsf(v_)) || (target * v_ < 0.0f); + const float amax = (slowing && amaxDec_ > 0.0f) ? amaxDec_ : amax_; + const float jmax = (slowing && jmaxDec_ > 0.0f) ? jmaxDec_ : jmax_; + + // Velocity still to be gained while bringing acceleration back to zero at the + // jerk limit. Aiming at the *predicted* velocity rather than the current one + // is what stops the profile overshooting its target. + const float vStop = a_ * fabsf(a_) / (2.0f * jmax); + const float vPred = v_ + vStop; + + // Acceleration that would close the remaining gap within this tick, saturated + // to the limit. Near the target this degrades smoothly instead of chattering + // between +/-amax. + const float aWanted = clampf((target - vPred) / dt, -amax, amax); + + const float dA = jmax * dt; + a_ += clampf(aWanted - a_, -dA, dA); + v_ += a_ * dt; + + // Anti-windup at the velocity ceiling. + if (v_ > vmax_) { v_ = vmax_; if (a_ > 0) a_ = 0; } + if (v_ < -vmax_) { v_ = -vmax_; if (a_ < 0) a_ = 0; } + + // Settle exactly, so a stopped robot reports a clean zero. + if (fabsf(target - v_) < 1e-4f && fabsf(a_) < 1e-3f) { + v_ = target; + a_ = 0; + } +} + +// --------------------------------------------------------------- Motion --- + +Motion::Motion() { + setResponse(response_); +} + +void Motion::setDecelBoost(float x) { + decelBoost_ = clampf(x, 1.0f, 6.0f); + setResponse(response_); // recompute both axes with the new ratio +} + +void Motion::setResponse(float r) { + response_ = clampf(r, 0.0f, 1.0f); + aMax_ = A_MAX_MPS2 + (A_MAX_SHARP - A_MAX_MPS2) * response_; + jMax_ = J_MAX_MPS3 + (J_MAX_SHARP - J_MAX_MPS3) * response_; + alphaMax_ = ALPHA_MAX + (ALPHA_MAX_SHARP - ALPHA_MAX) * response_; + jAlpha_ = J_MAX_ALPHA + (J_ALPHA_SHARP - J_MAX_ALPHA) * response_; + const float k = decelBoost_; + lin_.setLimits(aMax_, jMax_, aMax_ * k, jMax_ * k * k); + ang_.setLimits(alphaMax_, jAlpha_, alphaMax_ * k, jAlpha_ * k * k); +} + +void Motion::setDrive(float throttle, float steer) { + if (mode_ == Mode::EStop) return; + reqThrottle_ = clampf(throttle, -1.0f, 1.0f); + reqSteer_ = clampf(steer, -1.0f, 1.0f); + mode_ = Mode::Drive; +} + +void Motion::setVelocity(float mps, float radps) { + if (mode_ == Mode::EStop) return; + reqV_ = clampf(mps, -V_MAX, V_MAX); + reqW_ = clampf(radps, -W_MAX_RADS, W_MAX_RADS); + mode_ = Mode::Velocity; +} + +void Motion::moveDistance(float metres, const Odometry& odo) { + if (mode_ == Mode::EStop) return; + moveFrom_ = odo.distance(); + moveTo_ = moveFrom_ + metres; + holdRef_ = odo.heading(); // hold the heading we start with + remaining_ = lastRemaining_ = metres; + moveDir_ = (metres >= 0) ? 1 : -1; + stallTimer_ = 0; + mode_ = Mode::MoveLinear; +} + +void Motion::rotate(float radians, const Odometry& odo) { + if (mode_ == Mode::EStop) return; + moveFrom_ = odo.heading(); + moveTo_ = moveFrom_ + radians; + holdRef_ = odo.distance(); // hold position while turning + remaining_ = lastRemaining_ = radians; + moveDir_ = (radians >= 0) ? 1 : -1; + stallTimer_ = 0; + mode_ = Mode::MoveAngular; +} + +void Motion::stop() { + if (mode_ == Mode::EStop) return; + reqThrottle_ = reqSteer_ = 0; + reqV_ = reqW_ = 0; + mode_ = Mode::Idle; +} + +void Motion::emergencyStop() { + lin_.hardZero(); + ang_.hardZero(); + reqThrottle_ = reqSteer_ = reqV_ = reqW_ = 0; + cmdL_ = cmdR_ = 0; + mode_ = Mode::EStop; +} + +void Motion::clearEStop() { + if (mode_ == Mode::EStop) mode_ = Mode::Idle; +} + +float Motion::steerGain() const { + const float s = fabsf(lin_.velocity()) / V_MAX; + return STEER_GAIN_HIGH + (STEER_GAIN_LOW - STEER_GAIN_HIGH) * expf(-s / STEER_KNEE); +} + +void Motion::update(const Odometry& odo, float dt) { + if (mode_ == Mode::EStop) { + cmdL_ = cmdR_ = 0; + return; + } + + float vTarget = 0.0f; + float wTarget = 0.0f; + + switch (mode_) { + case Mode::Drive: + // The steering curve is evaluated against the speed the robot actually + // has right now, so authority fades in and out as it accelerates. + vTarget = reqThrottle_ * V_MAX; + wTarget = reqSteer_ * W_MAX_RADS * steerGain(); + break; + + case Mode::Velocity: + vTarget = reqV_; + wTarget = reqW_; + break; + + case Mode::MoveLinear: { + const double travelled = odo.distance() - moveFrom_; + remaining_ = moveTo_ - moveFrom_ - travelled; + + vTarget = approachVelocity((float)remaining_, 0.9f * aMax_ * decelBoost_, + jMax_ * decelBoost_ * decelBoost_, + overshot() ? MOVE_CREEP_MPS : MOVE_V_MAX, + MOVE_CREEP_MPS, MOVE_TOL_M); + + // Hold the starting heading: corrects the drift between two wheels that + // never quite match. + const float hErr = (float)(holdRef_ - odo.heading()); + wTarget = clampf(MOVE_HEADING_KP * hErr, -MOVE_TRIM_W_MAX, MOVE_TRIM_W_MAX); + + if (fabs(remaining_) < MOVE_TOL_M && fabsf(lin_.velocity()) < MOVE_SETTLE_MPS) { + mode_ = Mode::Idle; + vTarget = wTarget = 0; + } else if (stalled(dt)) { + vTarget = wTarget = 0; + } + break; + } + + case Mode::MoveAngular: { + const double turned = odo.heading() - moveFrom_; + remaining_ = moveTo_ - moveFrom_ - turned; + + wTarget = approachVelocity((float)remaining_, 0.9f * alphaMax_ * decelBoost_, + jAlpha_ * decelBoost_ * decelBoost_, + overshot() ? MOVE_CREEP_RADS : MOVE_W_MAX, + MOVE_CREEP_RADS, MOVE_TOL_RAD); + + // Hold station while spinning. + const float pErr = (float)(holdRef_ - odo.distance()); + vTarget = clampf(MOVE_POSITION_KP * pErr, -MOVE_TRIM_V_MAX, MOVE_TRIM_V_MAX); + + if (fabs(remaining_) < MOVE_TOL_RAD && fabsf(ang_.velocity()) < MOVE_SETTLE_RADS) { + mode_ = Mode::Idle; + vTarget = wTarget = 0; + } else if (stalled(dt)) { + vTarget = wTarget = 0; + } + break; + } + + case Mode::Idle: + case Mode::EStop: + default: + vTarget = wTarget = 0; + break; + } + + lin_.update(vTarget, dt); + ang_.update(wTarget, dt); + mixToWheels(); +} + +// Has the move gone past its target? Once it has, the correction is limited to +// creep speed, so an overshoot is walked back slowly instead of being answered +// with another full-speed run that overshoots the other way. +bool Motion::overshot() const { + return (remaining_ > 0) != (moveDir_ > 0); +} + +// A move that is not getting closer to its target is going nowhere useful: +// the feedback link has died, or something is physically in the way. Either +// way, stop rather than push indefinitely. +bool Motion::stalled(float dt) { + if (fabs(remaining_ - lastRemaining_) > MOVE_PROGRESS_EPS) { + lastRemaining_ = remaining_; + stallTimer_ = 0; + return false; + } + + stallTimer_ += dt; + if (stallTimer_ < MOVE_STALL_S) return false; + + mode_ = Mode::Idle; + aborted_ = true; + return true; +} + +void Motion::mixToWheels() { + const float half = 0.5f * TRACK_WIDTH_M; + float vl = lin_.velocity() - ang_.velocity() * half; + float vr = lin_.velocity() + ang_.velocity() * half; + + // If the pair asks for more than the wheels can give, scale both by the same + // factor: the robot goes slower but follows the same curve. + const float peak = fmaxf(fabsf(vl), fabsf(vr)); + if (peak > V_MAX) { + const float k = V_MAX / peak; + vl *= k; + vr *= k; + } + + cmdL_ = (int16_t)lroundf(vl / V_MAX * CMD_MAX); + cmdR_ = (int16_t)lroundf(vr / V_MAX * CMD_MAX); +} + +const char* Motion::modeName() const { + switch (mode_) { + case Mode::Idle: return "idle"; + case Mode::Drive: return "drive"; + case Mode::Velocity: return "velocity"; + case Mode::MoveLinear: return "move"; + case Mode::MoveAngular: return "rotate"; + case Mode::EStop: return "ESTOP"; + } + return "?"; +} diff --git a/esp32/.gitignore b/esp32/.gitignore new file mode 100644 index 0000000..968a41b --- /dev/null +++ b/esp32/.gitignore @@ -0,0 +1,2 @@ +.pio/ +.vscode/ diff --git a/esp32/include/hoverboard.h b/esp32/include/hoverboard.h new file mode 100644 index 0000000..7858acd --- /dev/null +++ b/esp32/include/hoverboard.h @@ -0,0 +1,89 @@ +// Serial link to a hoverboard mainboard running hoverboard-firmware-hack-FOC. +// +// Frames are the firmware's own structs (Inc/util.h, Src/main.c) — all int16, +// naturally packed, little-endian on both ends, XOR checksum. +#pragma once + +#include +#include "robot_config.h" + +constexpr uint16_t HOVER_START_FRAME = 0xABCD; // firmware SERIAL_START_FRAME + +// What we send. Under TANK_STEERING the firmware assigns: +// command.steer -> cmdL (left wheel), command.speed -> cmdR (right wheel). +// The field names below say what they actually drive. +struct __attribute__((packed)) HoverCommand { + uint16_t start; + int16_t left; + int16_t right; + // Mainboard buzzer: low byte = pitch (0 = silent), high byte = pattern. + // Level-triggered, so we own the duration; the mainboard's own fault beeps + // take priority over anything requested here. + uint16_t buzzer; + uint16_t checksum; +}; + +// What we receive. This layout assumes ENABLE_ODOMETRY is defined in the +// firmware; without it the two wheel counters are absent and the frame is four +// bytes shorter, which shows up here as a permanent checksum failure. +struct __attribute__((packed)) HoverFeedback { + uint16_t start; + int16_t cmd1; // command echo, left + int16_t cmd2; // command echo, right + int16_t speedR_meas; // rpm + int16_t speedL_meas; // rpm + int16_t wheelR_cnt; // hall ticks, wraps at ODOM_MODULO + int16_t wheelL_cnt; + int16_t batVoltage; // volts * 100 + int16_t boardTemp; // degrees C * 10 + uint16_t cmdLed; + uint16_t checksum; +}; + +class HoverboardLink { +public: + void begin(HardwareSerial& port); + + // Queue a command. Values are clamped to +/-CMD_MAX and wheel inversion from + // robot_config.h is applied here, so callers work in robot coordinates. + void send(int16_t left, int16_t right); + + // Sound the mainboard buzzer for `ms`, then clear it automatically. Pitch + // and pattern use the firmware's own scale: lower pitch value = higher tone, + // pattern 0 = continuous. Non-blocking. + void beep(uint8_t pitch, uint8_t pattern = 0, uint16_t ms = 120); + void beepOff(); + + // Drain the receive buffer. Returns true if a valid frame arrived this call. + bool poll(); + + const HoverFeedback& feedback() const { return fb_; } + uint32_t lastFrameMs() const { return lastFrameMs_; } + bool linkUp() const; + uint32_t framesOk() const { return framesOk_; } + uint32_t framesBad() const { return framesBad_; } + // Raw bytes seen on RX. Separates "the wire is silent" from "bytes are + // arriving but never frame up" (wrong baud, swapped pins, wrong layout). + uint32_t rxBytes() const { return rxBytes_; } + + float batteryVolts() const { return fb_.batVoltage / 100.0f; } + float boardTempC() const { return fb_.boardTemp / 10.0f; } + +private: + HardwareSerial* port_ = nullptr; + HoverFeedback fb_{}; + uint8_t buf_[sizeof(HoverFeedback)]{}; + size_t have_ = 0; + uint32_t lastFrameMs_ = 0; + uint32_t framesOk_ = 0; + uint32_t framesBad_ = 0; + uint32_t rxBytes_ = 0; + uint16_t buzzer_ = 0; + uint32_t beepUntilMs_ = 0; + + void dropByte(); +}; + +// Signed difference between two readings of a counter that wraps at +// ODOM_MODULO, taking the short way round. +int16_t hoverTickDelta(int16_t prev, int16_t curr); diff --git a/esp32/include/motion.h b/esp32/include/motion.h new file mode 100644 index 0000000..a0cf3ad --- /dev/null +++ b/esp32/include/motion.h @@ -0,0 +1,121 @@ +// Motion planning: jerk-limited body velocities, a speed-dependent steering +// curve, and closed-loop distance / rotation moves driven by wheel odometry. +#pragma once + +#include +#include "robot_config.h" + +// Wheel travel accumulated from the firmware's hall tick counters. +class Odometry { +public: + void reset(); // zero the accumulators + void update(int16_t rawLeft, int16_t rawRight); + + double left() const { return left_m_; } + double right() const { return right_m_; } + double distance() const { return 0.5 * (left_m_ + right_m_); } // metres + double heading() const { return (right_m_ - left_m_) / TRACK_WIDTH_M; } // radians, CCW+ + +private: + bool started_ = false; + int16_t prevL_ = 0, prevR_ = 0; + double left_m_ = 0, right_m_ = 0; +}; + +// One axis of a jerk-limited profile: acceleration itself is rate-limited, so +// the command has no step in its second derivative and the load never lurches. +class JerkAxis { +public: + JerkAxis(float vmax, float amax, float jmax) : vmax_(vmax), amax_(amax), jmax_(jmax) {} + + void update(float target, float dt); + void hardZero() { v_ = 0; a_ = 0; } + void setVMax(float vmax) { vmax_ = vmax; } + void setLimits(float amax, float jmax, float amaxDec, float jmaxDec) { + amax_ = amax; jmax_ = jmax; amaxDec_ = amaxDec; jmaxDec_ = jmaxDec; + } + float velocity() const { return v_; } + float accel() const { return a_; } + +private: + float v_ = 0, a_ = 0; + float vmax_, amax_, jmax_; + float amaxDec_ = 0, jmaxDec_ = 0; // 0 until setLimits: fall back to accel +}; + +class Motion { +public: + enum class Mode { Idle, Drive, Velocity, MoveLinear, MoveAngular, EStop }; + + // Pushes the default limits into both axes. Without this the axes start with + // no braking limits set and fall back to the acceleration ones, while the + // move planner already assumes the boost — it then brakes too late. + Motion(); + + // Operator-style input, -1..1 each. `steer` is scaled by the speed-dependent + // curve, so the same stick deflection spins on the spot at a standstill and + // only leans the robot at speed. + void setDrive(float throttle, float steer); + + // Programmatic input in physical units, no steering curve applied. + void setVelocity(float mps, float radps); + + // Closed-loop moves. Both return to Idle on arrival; busy() reports progress. + void moveDistance(float metres, const Odometry& odo); + void rotate(float radians, const Odometry& odo); + + // 0 = soft (long hauls), 1 = sharp (indoors). Scales acceleration and jerk + // on both axes; top speed is a separate setting. + void setResponse(float r); + float response() const { return response_; } + + // How much firmer braking is than acceleration. 1.0 = symmetric. + void setDecelBoost(float x); + float decelBoost() const { return decelBoost_; } + + void stop(); // ramp to zero through the jerk limits + void emergencyStop(); // zero the command this instant, then latch + void clearEStop(); + + void update(const Odometry& odo, float dt); + + // True once per abandoned move, so the caller can report it. + bool takeAbort() { const bool a = aborted_; aborted_ = false; return a; } + + int16_t cmdLeft() const { return cmdL_; } + int16_t cmdRight() const { return cmdR_; } + + Mode mode() const { return mode_; } + const char* modeName() const; + bool busy() const { return mode_ == Mode::MoveLinear || mode_ == Mode::MoveAngular; } + float linear() const { return lin_.velocity(); } + float angular() const { return ang_.velocity(); } + float steerGain() const; + double remaining() const { return remaining_; } + +private: + JerkAxis lin_{V_MAX, A_MAX_MPS2, J_MAX_MPS3}; + JerkAxis ang_{W_MAX_RADS, ALPHA_MAX, J_MAX_ALPHA}; + + Mode mode_ = Mode::Idle; + float reqThrottle_ = 0, reqSteer_ = 0; // Drive mode, normalised + float reqV_ = 0, reqW_ = 0; // Velocity mode, physical units + double moveFrom_ = 0, moveTo_ = 0; // metres or radians + double holdRef_ = 0; // heading (linear move) or distance (rotation) + double remaining_ = 0; + double lastRemaining_ = 0; // for stall detection + float stallTimer_ = 0; + bool aborted_ = false; + int moveDir_ = 1; // sign of the move as commanded + + float response_ = RESPONSE_DEFAULT; + float decelBoost_ = DECEL_BOOST_DEFAULT; + float aMax_ = A_MAX_MPS2, jMax_ = J_MAX_MPS3; + float alphaMax_ = ALPHA_MAX, jAlpha_ = J_MAX_ALPHA; + + int16_t cmdL_ = 0, cmdR_ = 0; + + void mixToWheels(); + bool stalled(float dt); + bool overshot() const; +}; diff --git a/esp32/include/robot_config.h b/esp32/include/robot_config.h new file mode 100644 index 0000000..4ecac36 --- /dev/null +++ b/esp32/include/robot_config.h @@ -0,0 +1,151 @@ +// Tuning and wiring for the TranspOtter drive controller. +// +// Everything a new chassis needs changing is in this file. The values marked +// MEASURE are geometry — the controller's distance and rotation commands are +// only as accurate as these. +#pragma once + +#include + +// ---------------------------------------------------------------- wiring --- +// UART1 to the hoverboard's RIGHT sideboard cable (USART3). +// +// GPIO2/8/9 are strapping pins on the C3 and GPIO8 also drives the Supermini's +// LED, so all three are out. GPIO20 is UART0's default RX, but the GPIO matrix +// routes UART1 here happily — just never call Serial0.begin(), which would +// claim GPIO20/21 and fight for the pin. GPIO21 is deliberately left alone: +// the ROM bootloader logs there at every reset, which would otherwise land in +// the mainboard's receiver. +constexpr int PIN_HOVER_RX = 20; // ESP32 RX <-- mainboard TX (blue, PB10) +constexpr int PIN_HOVER_TX = 10; // ESP32 TX --> mainboard RX (green, PB11) +constexpr uint32_t HOVER_BAUD = 115200; // must match USART3_BAUD in the firmware + +// ------------------------------------------------------------- geometry --- +constexpr float WHEEL_DIAMETER_M = 0.1651f; // 6.5" hub motor. MEASURE for other wheels. +constexpr float TRACK_WIDTH_M = 0.570f; // measured, wheel centre to wheel centre +constexpr int TICKS_PER_WHEEL_REV = 90; // 15 pole pairs x 6 hall states (firmware n_polePairs) +constexpr int ODOM_MODULO = 9000; // firmware wraps wheel_cnt at 9000 (= 100 revolutions) + +// Set true if a wheel counts or drives backwards. Calibrate once, on blocks, +// with `fwd 1` and `status` — see the README. +constexpr bool INVERT_LEFT = false; +constexpr bool INVERT_RIGHT = false; +constexpr bool INVERT_ODOM_LEFT = false; +constexpr bool INVERT_ODOM_RIGHT = false; + +// ------------------------------------------- agreement with the firmware --- +// These mirror Inc/config.h in hoverboard-firmware-hack-FOC. In SPD_MODE a +// command of +/-1000 is a closed-loop request for +/-N_MOT_MAX rpm, so the +// mapping from m/s to command units is exact. Change one side, change both. +constexpr float N_MOT_MAX_RPM = 200.0f; // firmware N_MOT_MAX +constexpr int CMD_MAX = 1000; // firmware INPUT_MAX + +// -------------------------------------------------------- motion limits --- +// Body-frame limits. The linear and angular axes are each jerk-limited, so a +// wheel never sees a step in acceleration — this is what keeps a heavy load +// from lurching at the start and nodding at the stop. +constexpr float A_MAX_MPS2 = 0.50f; // linear acceleration +constexpr float J_MAX_MPS3 = 3.00f; // linear jerk: how fast acceleration itself may change. + // 1.0 was too low to stop on a mark: reversing acceleration + // took a full second, and a closed-loop move sailed 160 mm + // past its target. 3.0 still ramps in over ~170 ms, so there + // is no step in acceleration and no lurch. +// Braking may be firmer than acceleration — a gentle start is what stops a +// load lurching, but a lazy stop just feels like slack in the controls. +// +// The multiplier scales the deceleration limit by x and the deceleration JERK +// by x*x. That second part is the one you feel: the delay before braking bites +// is amax/jmax, so squaring the jerk shortens that dead time by a factor of x +// rather than leaving it unchanged. +constexpr float DECEL_BOOST_DEFAULT = 2.0f; + +constexpr float W_MAX_RADS = 1.40f; // angular rate at full steer, standstill (~80 deg/s) +constexpr float ALPHA_MAX = 1.60f; // angular acceleration +constexpr float J_MAX_ALPHA = 9.60f; // angular jerk (same reasoning as J_MAX_MPS3) + +// The four limits above are the SOFT end of the response range — the tuning +// validated on the bench, and the right feel for hauling a load a long way. +// The values below are the SHARP end, for indoor work where reacting matters +// more than smoothness. The `response` setting (0..1, live-adjustable and +// saved to flash) interpolates between the two, so nothing here is a rebuild. +constexpr float A_MAX_SHARP = 1.50f; +constexpr float J_MAX_SHARP = 9.00f; +constexpr float ALPHA_MAX_SHARP = 3.50f; +constexpr float J_ALPHA_SHARP = 21.0f; + +// Startup defaults, overridden by whatever was last saved. +constexpr float RESPONSE_DEFAULT = 0.0f; // soft +constexpr float SPEED_LIMIT_DEFAULT = 0.4f; // 40% of V_MAX on the joystick + +// --------------------------------------------------------- steering feel --- +// Angular authority as a function of how fast the robot is already going: +// +// gain(s) = HIGH + (LOW - HIGH) * exp(-s / KNEE), s = |v| / V_MAX +// +// LOW applies at standstill, decaying towards HIGH as speed builds. KNEE is the +// fraction of top speed over which most of the extra authority disappears, so a +// small KNEE means "sharp only when crawling". +constexpr float STEER_GAIN_LOW = 1.00f; // full W_MAX when stationary — spins on the spot +constexpr float STEER_GAIN_HIGH = 0.22f; // at top speed, a gentle curve instead of a swerve +constexpr float STEER_KNEE = 0.15f; // authority is ~halved by 10% of top speed + +// ---------------------------------------------------- closed-loop moves --- +constexpr float MOVE_V_MAX = 0.35f; // m/s ceiling during `fwd` (leaves braking margin) +constexpr float MOVE_W_MAX = 0.70f; // rad/s ceiling during `rot` +constexpr float MOVE_TOL_M = 0.010f; // arrival tolerance, metres (~2 hall ticks) +constexpr float MOVE_TOL_RAD = 0.020f; // arrival tolerance, radians (~1.1 deg) +constexpr float MOVE_SETTLE_MPS = 0.02f; // below this a linear move counts as stopped +constexpr float MOVE_SETTLE_RADS = 0.05f; // below this a rotation counts as stopped + +// Floor on the approach speed. Without it the braking profile decays towards +// zero as the target nears and the last millimetres take forever. Both values +// are slow enough to stop inside the tolerance above. +constexpr float MOVE_CREEP_MPS = 0.030f; +constexpr float MOVE_CREEP_RADS = 0.050f; + +// ---------------------------------------------------------------- timing --- +constexpr uint32_t CONTROL_PERIOD_MS = 10; // 100 Hz command rate (firmware times out at 800 ms) +constexpr uint32_t FEEDBACK_TIMEOUT_MS = 500; // no valid frame for this long -> ramp down +constexpr uint32_t ARM_HOLD_MS = 1500; // hold zero at boot so the firmware arms the motors + +// Derived: top wheel speed the firmware will honour, in m/s. +constexpr float PI_F = 3.14159265f; +constexpr float V_MAX = (N_MOT_MAX_RPM / 60.0f) * PI_F * WHEEL_DIAMETER_M; + +// Trim gains for closed-loop moves: a straight `fwd` holds heading, a `rot` +// holds position. Both correct the small mismatch between the two wheels. +constexpr float MOVE_HEADING_KP = 1.5f; // rad/s per radian of heading error +constexpr float MOVE_POSITION_KP = 1.0f; // m/s per metre of drift +constexpr float MOVE_TRIM_W_MAX = 0.35f; // clamp on the heading correction +constexpr float MOVE_TRIM_V_MAX = 0.15f; // clamp on the position correction + +// Metres of travel per hall tick — the resolution of every closed-loop move. +constexpr float METRES_PER_TICK = (PI_F * WHEEL_DIAMETER_M) / TICKS_PER_WHEEL_REV; + +// ------------------------------------------------------------- remote UI --- +// The ESP32 hosts its own WiFi network and serves the control page at +// http://192.168.4.1 — no router and nothing to install. The password is WPA2 +// and must be at least 8 characters; an open network would let anyone within +// range drive the machine. +constexpr char AP_SSID[] = "transpotter"; +constexpr char AP_PASSWORD[] = "otterdrive"; +constexpr int AP_CHANNEL = 6; +constexpr int AP_MAX_CLIENTS = 2; + +// The browser sends a drive frame at 10 Hz, so it can afford a much tighter +// deadman than a human typing at the console. Walking out of radio range then +// ramps the machine down in well under a second. +constexpr uint32_t REMOTE_DEADMAN_MS = 400; + +// A closed-loop move ends by arriving, so it is exempt from the deadman below. +// That leaves it with no time limit at all, which is dangerous if the odometry +// stops advancing — a dropped link, or a wheel jammed against an obstacle. If +// the remaining distance fails to change by PROGRESS_EPS within STALL_S, the +// move is abandoned. +constexpr float MOVE_PROGRESS_EPS = 0.005f; // metres or radians +constexpr float MOVE_STALL_S = 1.5f; + +// A `drive` or `vel` command is a standing order; if nothing refreshes it +// within this long the controller ramps down. Closed-loop moves are exempt — +// they end by arriving. +constexpr uint32_t COMMAND_DEADMAN_MS = 2000; diff --git a/esp32/include/web_page.h b/esp32/include/web_page.h new file mode 100644 index 0000000..dae79e4 --- /dev/null +++ b/esp32/include/web_page.h @@ -0,0 +1,217 @@ +// The control page, served from flash. Kept deliberately small and +// dependency-free: no frameworks, no CDN, nothing to fetch. It has to work +// when the phone is joined to the machine's own network with no internet. +#pragma once + +static const char PAGE_HTML[] PROGMEM = R"HTML( + + + + +TranspOtter + + + +
+

TranspOtter

+ offline + idle + — V + — °C +
+ +
+ +
+ Speed + + 40% +
+ +
+ Response + + soft +
+ +
+ + + +
+ +
+ + +
+
+ + +
+ +
+
speed0.00
+
turn0.00
+
steer gain1.00
+
cmd L/R0/0
+
distance0.00
+
heading0.0
+
frames0
+
linkdown
+
+ + + +)HTML"; diff --git a/esp32/include/webui.h b/esp32/include/webui.h new file mode 100644 index 0000000..a54a81c --- /dev/null +++ b/esp32/include/webui.h @@ -0,0 +1,56 @@ +// WiFi access point + web UI for hand-driving the machine. +// +// The ESP32 hosts its own network; a browser at http://192.168.4.1 gets a +// touch joystick and live telemetry over a WebSocket. The page sends a drive +// frame at 10 Hz and the server answers each one with telemetry, so the +// request/response pairing doubles as the heartbeat — stop sending and the +// deadman ramps the machine down. +// +// The HTTP server runs in its own FreeRTOS task, so everything shared with the +// control loop goes through a spinlock. +#pragma once + +#include +#include "robot_config.h" + +namespace webui { + +// Snapshot the control loop publishes for the browser to read. +struct Telemetry { + const char* mode = "idle"; + int16_t cmdL = 0, cmdR = 0; + float v = 0, w = 0, gain = 1; + float dist = 0, headDeg = 0; + float battery = 0, temp = 0; + bool link = false, armed = false; + uint32_t framesOk = 0, framesBad = 0; + float limit = SPEED_LIMIT_DEFAULT; + float response = RESPONSE_DEFAULT; +}; + +struct DriveInput { + float throttle = 0, steer = 0; +}; + +struct Settings { + float limit = SPEED_LIMIT_DEFAULT; // joystick throttle scale, 0..1 + float response = RESPONSE_DEFAULT; // acceleration sharpness, 0..1 +}; + +void begin(); + +// Called every control tick. +void publish(const Telemetry& t); + +// True if a drive frame arrived since the last call. +bool takeDrive(DriveInput& out); + +// True if a one-shot command word arrived ("stop", "estop", "arm", "zero"). +bool takeCommand(char* buf, size_t len); + +// True if the browser changed the tuning sliders. +bool takeSettings(Settings& out); + +uint8_t clients(); + +} // namespace webui diff --git a/esp32/platformio.ini b/esp32/platformio.ini new file mode 100644 index 0000000..11ccd78 --- /dev/null +++ b/esp32/platformio.ini @@ -0,0 +1,34 @@ +; TranspOtter — ESP32-C3 Supermini motion controller for a hoverboard mainboard +; running hoverboard-firmware-hack-FOC (VARIANT_USART + TANK_STEERING + SPD_MODE). +; +; pio run build +; pio run -t upload flash over USB-C +; pio device monitor open the console (115200) + +[env:supermini] +platform = espressif32 +board = esp32-c3-devkitm-1 +framework = arduino + +monitor_speed = 115200 +monitor_filters = send_on_enter, colorize +; The C3's native USB-Serial-JTAG is not a real UART, so a baud change is both +; pointless and harmful — esptool's stub loses the link mid-handshake. 115200 is +; esptool's default, so it skips the renegotiation entirely. +upload_speed = 115200 + +; Reset over USB rather than toggling DTR/RTS like a UART bridge. Without this, +; a half-finished upload leaves the chip desynced and every later attempt hangs +; in esptool's `Connecting....` until it is physically unplugged. +board_upload.before_reset = usb_reset + +; Splices --no-stub into the esptool invocation; see the script for why. +extra_scripts = post:scripts/no_stub.py + +build_flags = + ; The Supermini's USB-C is the C3's native USB, so the console is USB CDC + ; and UART1 stays free for the hoverboard link. + -DARDUINO_USB_MODE=1 + -DARDUINO_USB_CDC_ON_BOOT=1 + -Wall + -Wextra diff --git a/esp32/scripts/no_stub.py b/esp32/scripts/no_stub.py new file mode 100644 index 0000000..7b9ebb3 --- /dev/null +++ b/esp32/scripts/no_stub.py @@ -0,0 +1,18 @@ +# Flash the ESP32-C3 with the ROM loader instead of esptool's RAM stub. +# +# Over the C3's native USB-Serial-JTAG the stub loads and starts, then the +# transfer stalls with "Packet content transfer stopped (received 0 bytes)" — +# reproducibly so through a USB hub. The ROM loader has no such trouble and is +# no slower here, because the link runs at USB speed regardless of the nominal +# baud rate. +# +# PlatformIO exposes --before/--after through board_upload.*, but not --no-stub, +# so it gets spliced into the argument list ahead of the write_flash subcommand +# (esptool requires global options to precede it). +Import("env") + +flags = list(env.get("UPLOADERFLAGS", [])) +if "write_flash" in flags and "--no-stub" not in flags: + flags.insert(flags.index("write_flash"), "--no-stub") + env.Replace(UPLOADERFLAGS=flags) + print("no_stub.py: using the ROM loader (--no-stub) for upload") diff --git a/esp32/src/hoverboard.cpp b/esp32/src/hoverboard.cpp new file mode 100644 index 0000000..be94d28 --- /dev/null +++ b/esp32/src/hoverboard.cpp @@ -0,0 +1,114 @@ +#include "hoverboard.h" + +namespace { + +// XOR of every 16-bit field except the checksum itself, matching +// usart_process_command() and the feedback packer in the firmware. +uint16_t checksumOf(const HoverCommand& c) { + return (uint16_t)(c.start ^ (uint16_t)c.left ^ (uint16_t)c.right ^ c.buzzer); +} + +uint16_t checksumOf(const HoverFeedback& f) { + return (uint16_t)(f.start ^ (uint16_t)f.cmd1 ^ (uint16_t)f.cmd2 ^ + (uint16_t)f.speedR_meas ^ (uint16_t)f.speedL_meas ^ + (uint16_t)f.wheelR_cnt ^ (uint16_t)f.wheelL_cnt ^ + (uint16_t)f.batVoltage ^ (uint16_t)f.boardTemp ^ f.cmdLed); +} + +int16_t clampCmd(int32_t v) { + if (v > CMD_MAX) return CMD_MAX; + if (v < -CMD_MAX) return -CMD_MAX; + return (int16_t)v; +} + +} // namespace + +void HoverboardLink::begin(HardwareSerial& port) { + port_ = &port; + port_->begin(HOVER_BAUD, SERIAL_8N1, PIN_HOVER_RX, PIN_HOVER_TX); +} + +void HoverboardLink::send(int16_t left, int16_t right) { + if (!port_) return; + + // Expire a finished beep here rather than on a timer, so the field is always + // consistent with the frame actually going out. + if (buzzer_ != 0 && (int32_t)(millis() - beepUntilMs_) >= 0) buzzer_ = 0; + + HoverCommand c; + c.start = HOVER_START_FRAME; + c.left = clampCmd(INVERT_LEFT ? -left : left); + c.right = clampCmd(INVERT_RIGHT ? -right : right); + c.buzzer = buzzer_; + c.checksum = checksumOf(c); + + port_->write((const uint8_t*)&c, sizeof(c)); +} + +void HoverboardLink::dropByte() { + // Resync: discard the oldest byte and let the next poll re-test the window. + if (have_ > 0) { + memmove(buf_, buf_ + 1, have_ - 1); + have_--; + } +} + +bool HoverboardLink::poll() { + if (!port_) return false; + bool got = false; + + while (port_->available() > 0) { + if (have_ < sizeof(buf_)) { + buf_[have_++] = (uint8_t)port_->read(); + rxBytes_++; + } + + // Keep the window anchored on a start frame before judging the checksum. + while (have_ >= 2) { + uint16_t start; + memcpy(&start, buf_, sizeof(start)); + if (start == HOVER_START_FRAME) break; + dropByte(); + } + + if (have_ == sizeof(buf_)) { + HoverFeedback candidate; + memcpy(&candidate, buf_, sizeof(candidate)); + + if (candidate.checksum == checksumOf(candidate)) { + fb_ = candidate; + lastFrameMs_ = millis(); + framesOk_++; + have_ = 0; + got = true; + } else { + // A corrupt or misaligned frame: step one byte and hunt again rather + // than throwing away bytes that may hold the real start marker. + framesBad_++; + dropByte(); + } + } + } + + return got; +} + +void HoverboardLink::beep(uint8_t pitch, uint8_t pattern, uint16_t ms) { + buzzer_ = (uint16_t)pitch | ((uint16_t)pattern << 8); + beepUntilMs_ = millis() + ms; +} + +void HoverboardLink::beepOff() { + buzzer_ = 0; +} + +bool HoverboardLink::linkUp() const { + return framesOk_ > 0 && (millis() - lastFrameMs_) < FEEDBACK_TIMEOUT_MS; +} + +int16_t hoverTickDelta(int16_t prev, int16_t curr) { + int32_t d = (int32_t)curr - (int32_t)prev; + d = ((d % ODOM_MODULO) + ODOM_MODULO) % ODOM_MODULO; // -> [0, ODOM_MODULO) + if (d >= ODOM_MODULO / 2) d -= ODOM_MODULO; // -> shortest signed path + return (int16_t)d; +} diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp new file mode 100644 index 0000000..efab770 --- /dev/null +++ b/esp32/src/main.cpp @@ -0,0 +1,347 @@ +// TranspOtter — ESP32-C3 Supermini motion controller. +// +// Talks to a hoverboard mainboard running hoverboard-firmware-hack-FOC +// (VARIANT_USART + TANK_STEERING + SPD_MODE) at 100 Hz, and exposes a line +// oriented console on USB for driving and for scripted moves. +// +// Type `help` in the monitor for the command list. + +#include +#include "robot_config.h" +#include "hoverboard.h" +#include "motion.h" +#include "webui.h" +#include + +namespace { + +HoverboardLink hoverLink; // not `link`: POSIX link() from makes that ambiguous +Odometry odo; +Motion motion; + +uint32_t bootMs = 0; +uint32_t lastCmdMs = 0; +uint32_t deadmanMs = COMMAND_DEADMAN_MS; // tightened while the web UI drives + +Preferences prefs; +float speedLimit = SPEED_LIMIT_DEFAULT; // joystick throttle scale +bool settingsDirty = false; +uint32_t settingsDirtyMs = 0; +bool armed = false; +bool linkWasUp = false; +bool telemetry = false; +uint32_t lastTelemMs = 0; + +String lineBuf; +SemaphoreHandle_t controlMux = nullptr; + +// ------------------------------------------------------------- console --- + +void printHelp() { + Serial.println(); + Serial.println(F("TranspOtter console")); + Serial.println(F(" drive -1..1 each; steer is speed-curved")); + Serial.println(F(" vel physical units, no steering curve")); + Serial.println(F(" fwd closed-loop straight move (holds heading)")); + Serial.println(F(" rot closed-loop spin on the spot (+ = left)")); + Serial.println(F(" stop ramp down through the jerk limits")); + Serial.println(F(" estop cut the command now, then latch")); + Serial.println(F(" arm clear an e-stop")); + Serial.println(F(" zero reset the odometry accumulators")); + Serial.println(F(" speed <0..1> joystick throttle scale (saved)")); + Serial.println(F(" resp <0..1> acceleration sharpness: 0 soft, 1 sharp (saved)")); + Serial.println(F(" decel <1..6> how much firmer braking is than acceleration (saved)")); + Serial.println(F(" tel [on|off] stream telemetry at 5 Hz")); + Serial.println(F(" status one-shot state dump")); + Serial.println(); +} + +void printStatus() { + const HoverFeedback& fb = hoverLink.feedback(); + Serial.printf("mode=%-8s armed=%d link=%d cmd=%+5d/%+5d v=%+.3f m/s w=%+.3f rad/s gain=%.2f\r\n", + motion.modeName(), armed ? 1 : 0, hoverLink.linkUp() ? 1 : 0, + motion.cmdLeft(), motion.cmdRight(), + motion.linear(), motion.angular(), motion.steerGain()); + Serial.printf("odo: L=%+.3f m R=%+.3f m dist=%+.3f m head=%+.1f deg\r\n", + odo.left(), odo.right(), odo.distance(), odo.heading() * 180.0 / PI_F); + Serial.printf("tuning: speed %.0f%% response %.2f decel %.2fx\r\n", + speedLimit * 100.0f, motion.response(), motion.decelBoost()); + Serial.printf("board: %.2f V %.1f C rpm L=%d R=%d frames ok=%lu bad=%lu rx=%lu bytes\r\n", + hoverLink.batteryVolts(), hoverLink.boardTempC(), + fb.speedL_meas, fb.speedR_meas, + (unsigned long)hoverLink.framesOk(), (unsigned long)hoverLink.framesBad(), + (unsigned long)hoverLink.rxBytes()); + if (motion.busy()) { + Serial.printf("move: %.3f remaining\r\n", motion.remaining()); + } +} + +void saveSettings() { + prefs.putFloat("limit", speedLimit); + prefs.putFloat("response", motion.response()); + prefs.putFloat("decel", motion.decelBoost()); + Serial.printf("saved: speed %.0f%% response %.2f\r\n", speedLimit * 100.0f, motion.response()); +} + +// Dragging a slider fires continuously, so settle first and write once — NVS +// has a finite number of erase cycles and there is no hurry. +void markSettingsDirty() { + settingsDirty = true; + settingsDirtyMs = millis(); +} + +bool applyWord(const char* w) { + if (!strcmp(w, "stop")) { + motion.stop(); + Serial.println(F("stopping")); + } else if (!strcmp(w, "estop")) { + motion.emergencyStop(); + Serial.println(F("E-STOP latched — send `arm` to clear")); + } else if (!strcmp(w, "arm")) { + motion.clearEStop(); + Serial.println(F("cleared")); + } else if (!strcmp(w, "zero")) { + odo.reset(); + Serial.println(F("odometry zeroed")); + } else { + return false; + } + return true; +} + +void handleLine(String line) { + line.trim(); + if (line.isEmpty()) return; + + // Split into a verb and up to two numeric arguments. + char verb[16] = {0}; + char word[16] = {0}; + float a1 = 0, a2 = 0; + const int n = sscanf(line.c_str(), "%15s %f %f", verb, &a1, &a2); + const int nWord = sscanf(line.c_str(), "%15s %15s", verb, word); + if (n < 1 && nWord < 1) return; + + const String cmd(verb); + + if (cmd == "help" || cmd == "?") { + printHelp(); + } else if (cmd == "status" || cmd == "st") { + printStatus(); + } else if (cmd == "drive") { + if (n < 3) { Serial.println(F("usage: drive ")); return; } + motion.setDrive(a1, a2); + lastCmdMs = millis(); + deadmanMs = COMMAND_DEADMAN_MS; + Serial.printf("drive %.2f %.2f\r\n", a1, a2); + } else if (cmd == "vel") { + if (n < 3) { Serial.println(F("usage: vel ")); return; } + motion.setVelocity(a1, a2); + lastCmdMs = millis(); + deadmanMs = COMMAND_DEADMAN_MS; + Serial.printf("vel %.3f %.3f\r\n", a1, a2); + } else if (cmd == "fwd") { + if (n < 2) { Serial.println(F("usage: fwd ")); return; } + if (!hoverLink.linkUp()) { Serial.println(F("refused: no odometry link")); return; } + motion.moveDistance(a1, odo); + Serial.printf("fwd %.3f m\r\n", a1); + } else if (cmd == "rot") { + if (n < 2) { Serial.println(F("usage: rot ")); return; } + if (!hoverLink.linkUp()) { Serial.println(F("refused: no odometry link")); return; } + motion.rotate(a1 * PI_F / 180.0f, odo); + Serial.printf("rot %.1f deg\r\n", a1); + } else if (applyWord(verb)) { + // stop / estop / arm / zero, handled in common with the web UI + } else if (cmd == "speed") { + if (n < 2) { Serial.printf("speed %.0f%%\r\n", speedLimit * 100.0f); return; } + speedLimit = constrain(a1, 0.05f, 1.0f); + markSettingsDirty(); + Serial.printf("speed %.0f%%\r\n", speedLimit * 100.0f); + } else if (cmd == "resp") { + if (n < 2) { Serial.printf("response %.2f\r\n", motion.response()); return; } + motion.setResponse(a1); + markSettingsDirty(); + Serial.printf("response %.2f\r\n", motion.response()); + } else if (cmd == "decel") { + if (n < 2) { Serial.printf("decel %.2fx\r\n", motion.decelBoost()); return; } + motion.setDecelBoost(a1); + markSettingsDirty(); + Serial.printf("decel %.2fx\r\n", motion.decelBoost()); + } else if (cmd == "tel") { + if (nWord < 2) telemetry = !telemetry; + else if (!strcmp(word, "on") || !strcmp(word, "1")) telemetry = true; + else if (!strcmp(word, "off") || !strcmp(word, "0")) telemetry = false; + else telemetry = !telemetry; + Serial.printf("telemetry %s\r\n", telemetry ? "on" : "off"); + } else { + Serial.printf("unknown command: %s (try `help`)\r\n", verb); + } +} + +void pumpConsole() { + while (Serial.available() > 0) { + const char c = (char)Serial.read(); + if (c == '\n' || c == '\r') { + if (lineBuf.length() > 0) { + handleLine(lineBuf); + lineBuf = ""; + } + } else if (lineBuf.length() < 96) { + lineBuf += c; + } + } +} + +// ----------------------------------------------------------- control --- + +// Everything with a deadline. Runs above the HTTP server (priority 4) and the +// Arduino loop task (priority 1): the mainboard cuts the motors if a command +// frame is over 0.8 s late, and neither a WiFi burst nor a Serial write that +// blocks on a slow terminal may be allowed to cause that. +void controlTick(uint32_t now, float dt) { + if (hoverLink.poll()) { + const HoverFeedback& fb = hoverLink.feedback(); + odo.update(fb.wheelL_cnt, fb.wheelR_cnt); + } + + // ---- web UI input ---- + webui::DriveInput remote; + if (webui::takeDrive(remote)) { + // The speed scale is applied here rather than in the browser, so a stale + // or misbehaving page cannot ask for more than the saved limit. + motion.setDrive(remote.throttle * speedLimit, remote.steer); + lastCmdMs = now; + deadmanMs = REMOTE_DEADMAN_MS; + } + + webui::Settings st; + if (webui::takeSettings(st)) { + speedLimit = st.limit; + motion.setResponse(st.response); + markSettingsDirty(); + } + if (settingsDirty && (now - settingsDirtyMs) > 1500) { + settingsDirty = false; + saveSettings(); + } + char word[16]; + if (webui::takeCommand(word, sizeof(word))) { + applyWord(word); + } + + if (!armed && (now - bootMs) >= ARM_HOLD_MS) { + armed = true; + Serial.println(F("armed — type `help` for commands")); + } + + // Losing feedback means we are driving blind: wind the command down rather + // than waiting for the mainboard's own 0.8 s timeout to cut the motors dead. + const bool up = hoverLink.linkUp(); + if (linkWasUp && !up) { + motion.stop(); + Serial.println(F("! feedback lost — ramping down")); + } else if (!linkWasUp && up) { + Serial.println(F("feedback link up")); + } + linkWasUp = up; + + // Deadman on standing velocity orders. Closed-loop moves end by arriving, so + // they are left alone. + if ((motion.mode() == Motion::Mode::Drive || motion.mode() == Motion::Mode::Velocity) && + (now - lastCmdMs) > deadmanMs) { + motion.stop(); + Serial.println(F("! command timeout — ramping down")); + } + + motion.update(odo, dt); + + if (motion.takeAbort()) { + Serial.println(F("! move abandoned — no odometry progress")); + } + + { + const HoverFeedback& fb = hoverLink.feedback(); + webui::Telemetry t; + t.mode = motion.modeName(); + t.cmdL = motion.cmdLeft(); + t.cmdR = motion.cmdRight(); + t.v = motion.linear(); + t.w = motion.angular(); + t.gain = motion.steerGain(); + t.dist = (float)odo.distance(); + t.headDeg = (float)(odo.heading() * 180.0 / PI_F); + t.battery = hoverLink.batteryVolts(); + t.temp = hoverLink.boardTempC(); + t.link = up; + t.armed = armed; + t.framesOk = hoverLink.framesOk(); + t.framesBad = hoverLink.framesBad(); + t.limit = speedLimit; + t.response = motion.response(); + (void)fb; + webui::publish(t); + } + + if (armed) { + hoverLink.send(motion.cmdLeft(), motion.cmdRight()); + } else { + hoverLink.send(0, 0); + } + +} + +void controlTask(void*) { + TickType_t last = xTaskGetTickCount(); + uint32_t prev = millis(); + for (;;) { + vTaskDelayUntil(&last, pdMS_TO_TICKS(CONTROL_PERIOD_MS)); + const uint32_t now = millis(); + const float dt = (now - prev) / 1000.0f; + prev = now; + if (xSemaphoreTake(controlMux, pdMS_TO_TICKS(50)) == pdTRUE) { + controlTick(now, dt > 0.0f ? dt : 0.001f); + xSemaphoreGive(controlMux); + } + } +} + +} // namespace + +void setup() { + Serial.begin(115200); + hoverLink.begin(Serial1); + prefs.begin("transpotter", false); + speedLimit = prefs.getFloat("limit", SPEED_LIMIT_DEFAULT); + motion.setDecelBoost(prefs.getFloat("decel", DECEL_BOOST_DEFAULT)); + motion.setResponse(prefs.getFloat("response", RESPONSE_DEFAULT)); + + webui::begin(); + + // Measured from here, not from power-on: bringing up WiFi takes a moment and + // the arming hold should cover the window in which we are actually sending. + bootMs = millis(); + lastCmdMs = bootMs; + + // The firmware only arms its motors once it has seen both inputs sit near + // zero, so hold a zero command over the arming window before doing anything. + Serial.println(); + controlMux = xSemaphoreCreateMutex(); + xTaskCreate(controlTask, "control", 4096, nullptr, 5, nullptr); + + Serial.println(F("TranspOtter controller starting — holding zero to arm the mainboard")); +} + +void loop() { + // Console and logging only; nothing here can delay a command frame. + pumpConsole(); + + const uint32_t now = millis(); + if (telemetry && (now - lastTelemMs) >= 200) { + lastTelemMs = now; + Serial.printf("t=%lu cmd=%+5d/%+5d v=%+.3f w=%+.3f dist=%+.3f head=%+.1f bat=%.2f\r\n", + (unsigned long)now, motion.cmdLeft(), motion.cmdRight(), + motion.linear(), motion.angular(), + odo.distance(), odo.heading() * 180.0 / PI_F, + hoverLink.batteryVolts()); + } + delay(2); +} diff --git a/esp32/src/motion.cpp b/esp32/src/motion.cpp new file mode 100644 index 0000000..84e174e --- /dev/null +++ b/esp32/src/motion.cpp @@ -0,0 +1,316 @@ +#include "motion.h" +#include "hoverboard.h" // hoverTickDelta +#include + +namespace { + +float clampf(float v, float lo, float hi) { + return v < lo ? lo : (v > hi ? hi : v); +} + +// Fastest approach speed from which the axis can still stop within `remaining`, +// given that deceleration cannot appear instantly but has to be ramped in at +// the jerk limit. +// +// Stopping from v costs v^2/(2a) while decelerating plus v*(a/j)/2 while the +// deceleration ramps up. Setting that equal to the distance left and solving +// for v gives the expression below. A plain sqrt(2*a*d) ignores the second +// term, which is exactly what makes a heavy robot sail past its mark. +float approachVelocity(float remaining, float amax, float jmax, + float vcap, float creep, float tol) { + const float d = fabsf(remaining); + if (d < tol) return 0.0f; + + // Twice the ramp time, not once: when the profile crosses from accelerating + // to braking, the axis is still at +amax and has to swing all the way to + // -amax, which is two jerk ramps rather than one. Budgeting for a single ramp + // is what let a move overshoot its target and then hunt back and forth. + const float tj = 2.0f * amax / jmax; + const float v = 0.5f * (-amax * tj + + sqrtf(amax * amax * tj * tj + 8.0f * amax * d)); + + return copysignf(fmaxf(fminf(v, vcap), creep), remaining); +} + +} // namespace + +// ------------------------------------------------------------- Odometry --- + +void Odometry::reset() { + left_m_ = right_m_ = 0; + started_ = false; +} + +void Odometry::update(int16_t rawLeft, int16_t rawRight) { + if (!started_) { + prevL_ = rawLeft; + prevR_ = rawRight; + started_ = true; + return; + } + + int16_t dL = hoverTickDelta(prevL_, rawLeft); + int16_t dR = hoverTickDelta(prevR_, rawRight); + prevL_ = rawLeft; + prevR_ = rawRight; + + if (INVERT_ODOM_LEFT) dL = -dL; + if (INVERT_ODOM_RIGHT) dR = -dR; + + left_m_ += dL * (double)METRES_PER_TICK; + right_m_ += dR * (double)METRES_PER_TICK; +} + +// ------------------------------------------------------------- JerkAxis --- + +void JerkAxis::update(float target, float dt) { + if (dt <= 0.0f) return; + + target = clampf(target, -vmax_, vmax_); + + // Slowing down, or reversing through zero, gets the firmer limits. + const bool slowing = (target == 0.0f) || (fabsf(target) < fabsf(v_)) || (target * v_ < 0.0f); + const float amax = (slowing && amaxDec_ > 0.0f) ? amaxDec_ : amax_; + const float jmax = (slowing && jmaxDec_ > 0.0f) ? jmaxDec_ : jmax_; + + // Velocity still to be gained while bringing acceleration back to zero at the + // jerk limit. Aiming at the *predicted* velocity rather than the current one + // is what stops the profile overshooting its target. + const float vStop = a_ * fabsf(a_) / (2.0f * jmax); + const float vPred = v_ + vStop; + + // Acceleration that would close the remaining gap within this tick, saturated + // to the limit. Near the target this degrades smoothly instead of chattering + // between +/-amax. + const float aWanted = clampf((target - vPred) / dt, -amax, amax); + + const float dA = jmax * dt; + a_ += clampf(aWanted - a_, -dA, dA); + v_ += a_ * dt; + + // Anti-windup at the velocity ceiling. + if (v_ > vmax_) { v_ = vmax_; if (a_ > 0) a_ = 0; } + if (v_ < -vmax_) { v_ = -vmax_; if (a_ < 0) a_ = 0; } + + // Settle exactly, so a stopped robot reports a clean zero. + if (fabsf(target - v_) < 1e-4f && fabsf(a_) < 1e-3f) { + v_ = target; + a_ = 0; + } +} + +// --------------------------------------------------------------- Motion --- + +Motion::Motion() { + setResponse(response_); +} + +void Motion::setDecelBoost(float x) { + decelBoost_ = clampf(x, 1.0f, 6.0f); + setResponse(response_); // recompute both axes with the new ratio +} + +void Motion::setResponse(float r) { + response_ = clampf(r, 0.0f, 1.0f); + aMax_ = A_MAX_MPS2 + (A_MAX_SHARP - A_MAX_MPS2) * response_; + jMax_ = J_MAX_MPS3 + (J_MAX_SHARP - J_MAX_MPS3) * response_; + alphaMax_ = ALPHA_MAX + (ALPHA_MAX_SHARP - ALPHA_MAX) * response_; + jAlpha_ = J_MAX_ALPHA + (J_ALPHA_SHARP - J_MAX_ALPHA) * response_; + const float k = decelBoost_; + lin_.setLimits(aMax_, jMax_, aMax_ * k, jMax_ * k * k); + ang_.setLimits(alphaMax_, jAlpha_, alphaMax_ * k, jAlpha_ * k * k); +} + +void Motion::setDrive(float throttle, float steer) { + if (mode_ == Mode::EStop) return; + reqThrottle_ = clampf(throttle, -1.0f, 1.0f); + reqSteer_ = clampf(steer, -1.0f, 1.0f); + mode_ = Mode::Drive; +} + +void Motion::setVelocity(float mps, float radps) { + if (mode_ == Mode::EStop) return; + reqV_ = clampf(mps, -V_MAX, V_MAX); + reqW_ = clampf(radps, -W_MAX_RADS, W_MAX_RADS); + mode_ = Mode::Velocity; +} + +void Motion::moveDistance(float metres, const Odometry& odo) { + if (mode_ == Mode::EStop) return; + moveFrom_ = odo.distance(); + moveTo_ = moveFrom_ + metres; + holdRef_ = odo.heading(); // hold the heading we start with + remaining_ = lastRemaining_ = metres; + moveDir_ = (metres >= 0) ? 1 : -1; + stallTimer_ = 0; + mode_ = Mode::MoveLinear; +} + +void Motion::rotate(float radians, const Odometry& odo) { + if (mode_ == Mode::EStop) return; + moveFrom_ = odo.heading(); + moveTo_ = moveFrom_ + radians; + holdRef_ = odo.distance(); // hold position while turning + remaining_ = lastRemaining_ = radians; + moveDir_ = (radians >= 0) ? 1 : -1; + stallTimer_ = 0; + mode_ = Mode::MoveAngular; +} + +void Motion::stop() { + if (mode_ == Mode::EStop) return; + reqThrottle_ = reqSteer_ = 0; + reqV_ = reqW_ = 0; + mode_ = Mode::Idle; +} + +void Motion::emergencyStop() { + lin_.hardZero(); + ang_.hardZero(); + reqThrottle_ = reqSteer_ = reqV_ = reqW_ = 0; + cmdL_ = cmdR_ = 0; + mode_ = Mode::EStop; +} + +void Motion::clearEStop() { + if (mode_ == Mode::EStop) mode_ = Mode::Idle; +} + +float Motion::steerGain() const { + const float s = fabsf(lin_.velocity()) / V_MAX; + return STEER_GAIN_HIGH + (STEER_GAIN_LOW - STEER_GAIN_HIGH) * expf(-s / STEER_KNEE); +} + +void Motion::update(const Odometry& odo, float dt) { + if (mode_ == Mode::EStop) { + cmdL_ = cmdR_ = 0; + return; + } + + float vTarget = 0.0f; + float wTarget = 0.0f; + + switch (mode_) { + case Mode::Drive: + // The steering curve is evaluated against the speed the robot actually + // has right now, so authority fades in and out as it accelerates. + vTarget = reqThrottle_ * V_MAX; + wTarget = reqSteer_ * W_MAX_RADS * steerGain(); + break; + + case Mode::Velocity: + vTarget = reqV_; + wTarget = reqW_; + break; + + case Mode::MoveLinear: { + const double travelled = odo.distance() - moveFrom_; + remaining_ = moveTo_ - moveFrom_ - travelled; + + vTarget = approachVelocity((float)remaining_, 0.9f * aMax_ * decelBoost_, + jMax_ * decelBoost_ * decelBoost_, + overshot() ? MOVE_CREEP_MPS : MOVE_V_MAX, + MOVE_CREEP_MPS, MOVE_TOL_M); + + // Hold the starting heading: corrects the drift between two wheels that + // never quite match. + const float hErr = (float)(holdRef_ - odo.heading()); + wTarget = clampf(MOVE_HEADING_KP * hErr, -MOVE_TRIM_W_MAX, MOVE_TRIM_W_MAX); + + if (fabs(remaining_) < MOVE_TOL_M && fabsf(lin_.velocity()) < MOVE_SETTLE_MPS) { + mode_ = Mode::Idle; + vTarget = wTarget = 0; + } else if (stalled(dt)) { + vTarget = wTarget = 0; + } + break; + } + + case Mode::MoveAngular: { + const double turned = odo.heading() - moveFrom_; + remaining_ = moveTo_ - moveFrom_ - turned; + + wTarget = approachVelocity((float)remaining_, 0.9f * alphaMax_ * decelBoost_, + jAlpha_ * decelBoost_ * decelBoost_, + overshot() ? MOVE_CREEP_RADS : MOVE_W_MAX, + MOVE_CREEP_RADS, MOVE_TOL_RAD); + + // Hold station while spinning. + const float pErr = (float)(holdRef_ - odo.distance()); + vTarget = clampf(MOVE_POSITION_KP * pErr, -MOVE_TRIM_V_MAX, MOVE_TRIM_V_MAX); + + if (fabs(remaining_) < MOVE_TOL_RAD && fabsf(ang_.velocity()) < MOVE_SETTLE_RADS) { + mode_ = Mode::Idle; + vTarget = wTarget = 0; + } else if (stalled(dt)) { + vTarget = wTarget = 0; + } + break; + } + + case Mode::Idle: + case Mode::EStop: + default: + vTarget = wTarget = 0; + break; + } + + lin_.update(vTarget, dt); + ang_.update(wTarget, dt); + mixToWheels(); +} + +// Has the move gone past its target? Once it has, the correction is limited to +// creep speed, so an overshoot is walked back slowly instead of being answered +// with another full-speed run that overshoots the other way. +bool Motion::overshot() const { + return (remaining_ > 0) != (moveDir_ > 0); +} + +// A move that is not getting closer to its target is going nowhere useful: +// the feedback link has died, or something is physically in the way. Either +// way, stop rather than push indefinitely. +bool Motion::stalled(float dt) { + if (fabs(remaining_ - lastRemaining_) > MOVE_PROGRESS_EPS) { + lastRemaining_ = remaining_; + stallTimer_ = 0; + return false; + } + + stallTimer_ += dt; + if (stallTimer_ < MOVE_STALL_S) return false; + + mode_ = Mode::Idle; + aborted_ = true; + return true; +} + +void Motion::mixToWheels() { + const float half = 0.5f * TRACK_WIDTH_M; + float vl = lin_.velocity() - ang_.velocity() * half; + float vr = lin_.velocity() + ang_.velocity() * half; + + // If the pair asks for more than the wheels can give, scale both by the same + // factor: the robot goes slower but follows the same curve. + const float peak = fmaxf(fabsf(vl), fabsf(vr)); + if (peak > V_MAX) { + const float k = V_MAX / peak; + vl *= k; + vr *= k; + } + + cmdL_ = (int16_t)lroundf(vl / V_MAX * CMD_MAX); + cmdR_ = (int16_t)lroundf(vr / V_MAX * CMD_MAX); +} + +const char* Motion::modeName() const { + switch (mode_) { + case Mode::Idle: return "idle"; + case Mode::Drive: return "drive"; + case Mode::Velocity: return "velocity"; + case Mode::MoveLinear: return "move"; + case Mode::MoveAngular: return "rotate"; + case Mode::EStop: return "ESTOP"; + } + return "?"; +} diff --git a/esp32/src/webui.cpp b/esp32/src/webui.cpp new file mode 100644 index 0000000..4de4fa7 --- /dev/null +++ b/esp32/src/webui.cpp @@ -0,0 +1,181 @@ +#include "webui.h" +#include "web_page.h" +#include "robot_config.h" + +#include +#include + +namespace webui { +namespace { + +httpd_handle_t server = nullptr; + +// Everything below is touched by both the HTTP task and the control loop. +portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; + +Telemetry shared{}; +DriveInput pendingDrive{}; +bool haveDrive = false; +char pendingCmd[16] = {0}; +bool haveCmd = false; +Settings pendingSettings{}; +bool haveSettings = false; + +float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); } + +esp_err_t indexHandler(httpd_req_t* req) { + httpd_resp_set_type(req, "text/html"); + httpd_resp_set_hdr(req, "Cache-Control", "no-store"); + return httpd_resp_send(req, PAGE_HTML, HTTPD_RESP_USE_STRLEN); +} + +int buildTelemetry(char* out, size_t len) { + Telemetry t; + portENTER_CRITICAL(&mux); + t = shared; + portEXIT_CRITICAL(&mux); + + return snprintf(out, len, + "{\"m\":\"%s\",\"a\":%d,\"k\":%d,\"cl\":%d,\"cr\":%d," + "\"v\":%.3f,\"w\":%.3f,\"g\":%.2f,\"d\":%.3f,\"h\":%.1f," + "\"b\":%.2f,\"t\":%.1f,\"fo\":%lu,\"fb\":%lu," + "\"lim\":%.2f,\"rsp\":%.2f}", + t.mode, t.armed ? 1 : 0, t.link ? 1 : 0, t.cmdL, t.cmdR, + t.v, t.w, t.gain, t.dist, t.headDeg, + t.battery, t.temp, + (unsigned long)t.framesOk, (unsigned long)t.framesBad, + t.limit, t.response); +} + +esp_err_t wsHandler(httpd_req_t* req) { + if (req->method == HTTP_GET) return ESP_OK; // handshake only + + httpd_ws_frame_t in{}; + in.type = HTTPD_WS_TYPE_TEXT; + + // First call with len 0 reports the frame size. + esp_err_t err = httpd_ws_recv_frame(req, &in, 0); + if (err != ESP_OK) return err; + if (in.len == 0 || in.len > 64) return ESP_OK; + + uint8_t buf[65] = {0}; + in.payload = buf; + err = httpd_ws_recv_frame(req, &in, in.len); + if (err != ESP_OK) return err; + + if (buf[0] == 'd') { // d,, + float th = 0, st = 0; + if (sscanf((const char*)buf, "d,%f,%f", &th, &st) == 2) { + portENTER_CRITICAL(&mux); + pendingDrive.throttle = clampf(th, -1.0f, 1.0f); + pendingDrive.steer = clampf(st, -1.0f, 1.0f); + haveDrive = true; + portEXIT_CRITICAL(&mux); + } + } else if (buf[0] == 's') { // s,, + float lim = 0, rsp = 0; + if (sscanf((const char*)buf, "s,%f,%f", &lim, &rsp) == 2) { + portENTER_CRITICAL(&mux); + pendingSettings.limit = clampf(lim, 0.05f, 1.0f); + pendingSettings.response = clampf(rsp, 0.0f, 1.0f); + haveSettings = true; + portEXIT_CRITICAL(&mux); + } + } else if (buf[0] == 'c') { // c, + char word[16] = {0}; + if (sscanf((const char*)buf, "c,%15s", word) == 1) { + portENTER_CRITICAL(&mux); + strncpy(pendingCmd, word, sizeof(pendingCmd) - 1); + pendingCmd[sizeof(pendingCmd) - 1] = '\0'; + haveCmd = true; + portEXIT_CRITICAL(&mux); + } + } + + // Answering every frame keeps the browser's 10 Hz cadence as the heartbeat + // and avoids the bookkeeping of async sends to tracked sockets. + char json[320]; + const int n = buildTelemetry(json, sizeof(json)); + if (n <= 0) return ESP_OK; + + httpd_ws_frame_t out{}; + out.type = HTTPD_WS_TYPE_TEXT; + out.payload = (uint8_t*)json; + out.len = (size_t)n; + return httpd_ws_send_frame(req, &out); +} + +} // namespace + +void begin() { + WiFi.mode(WIFI_AP); + WiFi.softAP(AP_SSID, AP_PASSWORD, AP_CHANNEL, false, AP_MAX_CLIENTS); + + httpd_config_t cfg = HTTPD_DEFAULT_CONFIG(); + // Below the Arduino loop task would starve the 100 Hz control loop under + // load; above it, a burst of HTTP work could delay a command frame. The + // mainboard closes its own loops at 16 kHz, so a late setpoint is harmless, + // but there is no reason to make it likely. + cfg.task_priority = 4; + cfg.stack_size = 4096; + cfg.max_open_sockets = 4; + cfg.lru_purge_enable = true; + + if (httpd_start(&server, &cfg) != ESP_OK) { + Serial.println(F("! web UI: httpd_start failed")); + server = nullptr; + return; + } + + httpd_uri_t page{}; + page.uri = "/"; + page.method = HTTP_GET; + page.handler = indexHandler; + page.user_ctx = nullptr; + httpd_register_uri_handler(server, &page); + + httpd_uri_t ws{}; + ws.uri = "/ws"; + ws.method = HTTP_GET; + ws.handler = wsHandler; + ws.user_ctx = nullptr; + ws.is_websocket = true; + httpd_register_uri_handler(server, &ws); + + Serial.printf("web UI: join WiFi \"%s\" (password %s), then open http://%s\r\n", + AP_SSID, AP_PASSWORD, WiFi.softAPIP().toString().c_str()); +} + +void publish(const Telemetry& t) { + portENTER_CRITICAL(&mux); + shared = t; + portEXIT_CRITICAL(&mux); +} + +bool takeDrive(DriveInput& out) { + bool got = false; + portENTER_CRITICAL(&mux); + if (haveDrive) { out = pendingDrive; haveDrive = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +bool takeCommand(char* buf, size_t len) { + bool got = false; + portENTER_CRITICAL(&mux); + if (haveCmd) { strncpy(buf, pendingCmd, len - 1); buf[len - 1] = '\0'; haveCmd = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +bool takeSettings(Settings& out) { + bool got = false; + portENTER_CRITICAL(&mux); + if (haveSettings) { out = pendingSettings; haveSettings = false; got = true; } + portEXIT_CRITICAL(&mux); + return got; +} + +uint8_t clients() { return WiFi.softAPgetStationNum(); } + +} // namespace webui diff --git a/hoverboard-firmware-hack-FOC b/hoverboard-firmware-hack-FOC new file mode 160000 index 0000000..4f141cb --- /dev/null +++ b/hoverboard-firmware-hack-FOC @@ -0,0 +1 @@ +Subproject commit 4f141cbc97b297e194fd58e56444bd0322f902ac