First commit
This commit is contained in:
@@ -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 <throttle> <steer>` | −1..1 each; `steer` goes through the speed curve |
|
||||
| `vel <m/s> <rad/s>` | physical units, no steering curve — the programmatic path |
|
||||
| `fwd <metres>` | closed-loop straight move |
|
||||
| `rot <degrees>` | 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.
|
||||
@@ -0,0 +1,2 @@
|
||||
.pio/
|
||||
.vscode/
|
||||
Executable
+154
@@ -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_EOF
|
||||
# Written by $(basename "$0") — see that script for why these matter.
|
||||
pu port $PORT
|
||||
pu baudrate $BAUD
|
||||
pu bits 8
|
||||
pu parity N
|
||||
pu stopbits 1
|
||||
pu rtscts No
|
||||
pu xonxoff No
|
||||
pu localecho Yes
|
||||
pu addcarreturn Yes
|
||||
PROFILE_EOF
|
||||
)
|
||||
# Rewrite whenever it differs, so a stale profile cannot quietly reintroduce
|
||||
# any of the problems above.
|
||||
if [ ! -f "$PROFILE" ] || [ "$(cat "$PROFILE")" != "$WANT" ]; then
|
||||
printf '%s\n' "$WANT" > "$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"
|
||||
@@ -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 <Arduino.h>
|
||||
#include <NimBLEDevice.h>
|
||||
|
||||
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
|
||||
@@ -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 <Arduino.h>
|
||||
#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);
|
||||
@@ -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 <Arduino.h>
|
||||
#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;
|
||||
};
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
// ---------------------------------------------------------------- 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;
|
||||
@@ -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
|
||||
@@ -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")
|
||||
@@ -0,0 +1,509 @@
|
||||
#include "ble_pad.h"
|
||||
#include "robot_config.h"
|
||||
#include <vector>
|
||||
|
||||
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<Found> 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 <index>`\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 <fn> 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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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 <Arduino.h>
|
||||
#include <Preferences.h>
|
||||
|
||||
#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 <index|addr> 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 <fn> [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 <fn> <byte> <mask> <value> 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 <ax> <byte> <centre> <lo> <hi> [deadband] [invert]"));
|
||||
Serial.println(F(" clearaxes wipe axis bindings"));
|
||||
Serial.println(F(" inject <hex bytes> 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 <ms> buttons count as released after this long with no report"));
|
||||
Serial.println(F(" pulse <ms> drive time per click in pulse mode"));
|
||||
Serial.println(F(" idle <sec> 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 <pitch> [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 <index|address>")); 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 <fwd|back|left|right|brake|turbo>")); 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 <ax> <byte> <centre> <lo> <hi> [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 <fn> <byte> <mask> <value> 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);
|
||||
}
|
||||
@@ -0,0 +1,316 @@
|
||||
#include "motion.h"
|
||||
#include "hoverboard.h" // hoverTickDelta
|
||||
#include <math.h>
|
||||
|
||||
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 "?";
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
.pio/
|
||||
.vscode/
|
||||
@@ -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 <Arduino.h>
|
||||
#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);
|
||||
@@ -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 <Arduino.h>
|
||||
#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;
|
||||
};
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
// ---------------------------------------------------------------- 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;
|
||||
@@ -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(<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1,user-scalable=no,viewport-fit=cover">
|
||||
<title>TranspOtter</title>
|
||||
<style>
|
||||
:root{
|
||||
--bg:#0f1214; --panel:#181d21; --line:#2a3238; --text:#e6edf3;
|
||||
--dim:#8b979f; --accent:#4cc2ff; --ok:#3fb950; --warn:#d29922; --bad:#f85149;
|
||||
}
|
||||
*{box-sizing:border-box;-webkit-tap-highlight-color:transparent}
|
||||
html,body{margin:0;height:100%}
|
||||
body{
|
||||
background:var(--bg);color:var(--text);
|
||||
font:14px/1.4 ui-sans-serif,system-ui,-apple-system,"Segoe UI",Roboto,sans-serif;
|
||||
display:flex;flex-direction:column;gap:10px;padding:10px;
|
||||
padding-bottom:calc(10px + env(safe-area-inset-bottom));
|
||||
overscroll-behavior:none;touch-action:none;
|
||||
}
|
||||
header{display:flex;align-items:center;gap:10px;flex-wrap:wrap}
|
||||
h1{font-size:15px;margin:0;font-weight:600;letter-spacing:.02em}
|
||||
.pill{
|
||||
display:inline-flex;align-items:center;gap:6px;background:var(--panel);
|
||||
border:1px solid var(--line);border-radius:999px;padding:4px 10px;
|
||||
font-variant-numeric:tabular-nums;font-size:12px;
|
||||
}
|
||||
.dot{width:8px;height:8px;border-radius:50%;background:var(--bad)}
|
||||
.dot.on{background:var(--ok)}
|
||||
.pad{
|
||||
flex:1;min-height:210px;background:var(--panel);border:1px solid var(--line);
|
||||
border-radius:16px;position:relative;overflow:hidden;touch-action:none;
|
||||
}
|
||||
.pad canvas{width:100%;height:100%;display:block}
|
||||
.row{display:flex;gap:8px}
|
||||
.row>*{flex:1}
|
||||
button{
|
||||
background:var(--panel);color:var(--text);border:1px solid var(--line);
|
||||
border-radius:12px;padding:13px 8px;font-size:14px;font-weight:600;
|
||||
font-family:inherit;cursor:pointer;
|
||||
}
|
||||
button:active{background:#232a30}
|
||||
button.stop{border-color:#5a4a1f;color:#f0c674}
|
||||
button.estop{background:#3d1519;border-color:#7d2b31;color:#ff9c96}
|
||||
.grid{
|
||||
display:grid;grid-template-columns:repeat(auto-fit,minmax(88px,1fr));gap:6px;
|
||||
background:var(--panel);border:1px solid var(--line);border-radius:12px;padding:10px;
|
||||
}
|
||||
.cell{display:flex;flex-direction:column;gap:2px}
|
||||
.k{color:var(--dim);font-size:10px;text-transform:uppercase;letter-spacing:.06em}
|
||||
.val{font-variant-numeric:tabular-nums;font-size:15px;font-weight:600}
|
||||
.limit{display:flex;align-items:center;gap:10px;background:var(--panel);
|
||||
border:1px solid var(--line);border-radius:12px;padding:8px 12px}
|
||||
input[type=range]{flex:1;accent-color:var(--accent)}
|
||||
@media (min-width:720px){ body{max-width:760px;margin:0 auto} }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<h1>TranspOtter</h1>
|
||||
<span class="pill"><span id="dot" class="dot"></span><span id="conn">offline</span></span>
|
||||
<span class="pill" id="mode">idle</span>
|
||||
<span class="pill" id="batt">— V</span>
|
||||
<span class="pill" id="temp">— °C</span>
|
||||
</header>
|
||||
|
||||
<div class="pad" id="pad"><canvas id="cv"></canvas></div>
|
||||
|
||||
<div class="limit">
|
||||
<span class="k">Speed</span>
|
||||
<input type="range" id="lim" min="5" max="100" value="40">
|
||||
<span class="val" id="limv" style="min-width:3.2em;text-align:right">40%</span>
|
||||
</div>
|
||||
|
||||
<div class="limit">
|
||||
<span class="k">Response</span>
|
||||
<input type="range" id="rsp" min="0" max="100" value="0">
|
||||
<span class="val" id="rspv" style="min-width:3.2em;text-align:right">soft</span>
|
||||
</div>
|
||||
|
||||
<div class="row">
|
||||
<button id="p0">HAUL</button>
|
||||
<button id="p1">NORMAL</button>
|
||||
<button id="p2">INDOOR</button>
|
||||
</div>
|
||||
|
||||
<div class="row">
|
||||
<button class="stop" id="bstop">STOP</button>
|
||||
<button class="estop" id="bestop">E-STOP</button>
|
||||
</div>
|
||||
<div class="row">
|
||||
<button id="barm">ARM</button>
|
||||
<button id="bzero">ZERO ODO</button>
|
||||
</div>
|
||||
|
||||
<div class="grid">
|
||||
<div class="cell"><span class="k">speed</span><span class="val" id="v">0.00</span></div>
|
||||
<div class="cell"><span class="k">turn</span><span class="val" id="w">0.00</span></div>
|
||||
<div class="cell"><span class="k">steer gain</span><span class="val" id="g">1.00</span></div>
|
||||
<div class="cell"><span class="k">cmd L/R</span><span class="val" id="c">0/0</span></div>
|
||||
<div class="cell"><span class="k">distance</span><span class="val" id="d">0.00</span></div>
|
||||
<div class="cell"><span class="k">heading</span><span class="val" id="h">0.0</span></div>
|
||||
<div class="cell"><span class="k">frames</span><span class="val" id="f">0</span></div>
|
||||
<div class="cell"><span class="k">link</span><span class="val" id="lk">down</span></div>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
const $=id=>document.getElementById(id);
|
||||
let ws=null, wsOpen=false;
|
||||
let jx=0, jy=0, limit=0.4, active=false;
|
||||
|
||||
// ---- joystick -------------------------------------------------------------
|
||||
const pad=$('pad'), cv=$('cv'), ctx=cv.getContext('2d');
|
||||
let W=0,H=0,R=0,cx=0,cy=0,dpr=1;
|
||||
|
||||
function size(){
|
||||
dpr=window.devicePixelRatio||1;
|
||||
W=pad.clientWidth; H=pad.clientHeight;
|
||||
cv.width=W*dpr; cv.height=H*dpr; ctx.setTransform(dpr,0,0,dpr,0,0);
|
||||
cx=W/2; cy=H/2; R=Math.min(W,H)/2-16;
|
||||
draw();
|
||||
}
|
||||
function draw(){
|
||||
ctx.clearRect(0,0,W,H);
|
||||
ctx.strokeStyle='#2a3238'; ctx.lineWidth=1;
|
||||
ctx.beginPath(); ctx.arc(cx,cy,R,0,6.2832); ctx.stroke();
|
||||
ctx.beginPath(); ctx.moveTo(cx-R,cy); ctx.lineTo(cx+R,cy);
|
||||
ctx.moveTo(cx,cy-R); ctx.lineTo(cx,cy+R); ctx.stroke();
|
||||
ctx.strokeStyle='#212930';
|
||||
ctx.beginPath(); ctx.arc(cx,cy,R*0.5,0,6.2832); ctx.stroke();
|
||||
const kx=cx+jx*R, ky=cy-jy*R;
|
||||
ctx.strokeStyle='#4cc2ff'; ctx.lineWidth=2;
|
||||
ctx.beginPath(); ctx.moveTo(cx,cy); ctx.lineTo(kx,ky); ctx.stroke();
|
||||
ctx.fillStyle=active?'#4cc2ff':'#5a666e';
|
||||
ctx.beginPath(); ctx.arc(kx,ky,20,0,6.2832); ctx.fill();
|
||||
}
|
||||
function at(e){
|
||||
const r=pad.getBoundingClientRect();
|
||||
let x=(e.clientX-r.left-cx)/R, y=-(e.clientY-r.top-cy)/R;
|
||||
const m=Math.hypot(x,y); if(m>1){x/=m;y/=m;}
|
||||
jx=x; jy=y; draw();
|
||||
}
|
||||
pad.addEventListener('pointerdown',e=>{active=true;pad.setPointerCapture(e.pointerId);at(e);});
|
||||
pad.addEventListener('pointermove',e=>{if(active)at(e);});
|
||||
function release(){active=false;jx=0;jy=0;draw();}
|
||||
pad.addEventListener('pointerup',release);
|
||||
pad.addEventListener('pointercancel',release);
|
||||
window.addEventListener('resize',size);
|
||||
|
||||
// Speed scales top end; response scales how hard it accelerates to get there.
|
||||
// Both live on the device and are saved there, so a fresh phone picks up the
|
||||
// settings you last used rather than imposing its own.
|
||||
let response=0, synced=false;
|
||||
function rspLabel(v){ return v<15?'soft':v<40?'easy':v<70?'brisk':v<90?'sharp':'max'; }
|
||||
function pushSettings(){ send('s,'+limit.toFixed(2)+','+response.toFixed(2)); }
|
||||
$('lim').addEventListener('input',e=>{
|
||||
limit=e.target.value/100; $('limv').textContent=e.target.value+'%'; pushSettings();
|
||||
});
|
||||
$('rsp').addEventListener('input',e=>{
|
||||
response=e.target.value/100; $('rspv').textContent=rspLabel(+e.target.value); pushSettings();
|
||||
});
|
||||
function preset(v){
|
||||
response=v/100; $('rsp').value=v; $('rspv').textContent=rspLabel(v); pushSettings();
|
||||
}
|
||||
$('p0').onclick=()=>preset(0);
|
||||
$('p1').onclick=()=>preset(45);
|
||||
$('p2').onclick=()=>preset(100);
|
||||
|
||||
// ---- link -----------------------------------------------------------------
|
||||
function connect(){
|
||||
ws=new WebSocket('ws://'+location.host+'/ws');
|
||||
ws.onopen =()=>{wsOpen=true;synced=false;$('dot').classList.add('on');$('conn').textContent='online';};
|
||||
ws.onclose=()=>{wsOpen=false;$('dot').classList.remove('on');$('conn').textContent='offline';
|
||||
setTimeout(connect,700);};
|
||||
ws.onerror=()=>ws.close();
|
||||
ws.onmessage=e=>{
|
||||
let t; try{t=JSON.parse(e.data);}catch(_){return;}
|
||||
$('mode').textContent=t.m;
|
||||
$('batt').textContent=t.b.toFixed(1)+' V';
|
||||
$('temp').textContent=t.t.toFixed(0)+' °C';
|
||||
$('v').textContent=t.v.toFixed(2);
|
||||
$('w').textContent=t.w.toFixed(2);
|
||||
$('g').textContent=t.g.toFixed(2);
|
||||
$('c').textContent=t.cl+'/'+t.cr;
|
||||
$('d').textContent=t.d.toFixed(2);
|
||||
$('h').textContent=t.h.toFixed(1);
|
||||
$('f').textContent=t.fo+(t.fb?(' / '+t.fb+' bad'):'');
|
||||
$('lk').textContent=t.k?'up':'down';
|
||||
$('lk').style.color=t.k?'var(--ok)':'var(--bad)';
|
||||
if(!synced && t.lim!==undefined){ // adopt the device's saved values once
|
||||
synced=true;
|
||||
limit=t.lim; $('lim').value=Math.round(t.lim*100); $('limv').textContent=Math.round(t.lim*100)+'%';
|
||||
response=t.rsp; $('rsp').value=Math.round(t.rsp*100); $('rspv').textContent=rspLabel(Math.round(t.rsp*100));
|
||||
}
|
||||
};
|
||||
}
|
||||
function send(s){ if(wsOpen) try{ws.send(s);}catch(_){ } }
|
||||
function cmd(w){ send('c,'+w); }
|
||||
$('bstop').onclick =()=>{release();cmd('stop');};
|
||||
$('bestop').onclick=()=>{release();cmd('estop');};
|
||||
$('barm').onclick =()=>cmd('arm');
|
||||
$('bzero').onclick =()=>cmd('zero');
|
||||
|
||||
// The drive frame is also the heartbeat: it goes out at 10 Hz whether or not
|
||||
// the stick moved. Stop sending — background the tab, walk out of range — and
|
||||
// the firmware's deadman ramps the machine down.
|
||||
setInterval(()=>send('d,'+jy.toFixed(3)+','+jx.toFixed(3)),100);
|
||||
|
||||
size(); connect();
|
||||
</script>
|
||||
</body>
|
||||
</html>)HTML";
|
||||
@@ -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 <Arduino.h>
|
||||
#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
|
||||
@@ -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
|
||||
@@ -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")
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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 <Arduino.h>
|
||||
#include "robot_config.h"
|
||||
#include "hoverboard.h"
|
||||
#include "motion.h"
|
||||
#include "webui.h"
|
||||
#include <Preferences.h>
|
||||
|
||||
namespace {
|
||||
|
||||
HoverboardLink hoverLink; // not `link`: POSIX link() from <unistd.h> 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 <throttle> <steer> -1..1 each; steer is speed-curved"));
|
||||
Serial.println(F(" vel <m/s> <rad/s> physical units, no steering curve"));
|
||||
Serial.println(F(" fwd <metres> closed-loop straight move (holds heading)"));
|
||||
Serial.println(F(" rot <degrees> 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 <throttle -1..1> <steer -1..1>")); 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 <m/s> <rad/s>")); 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 <metres>")); 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 <degrees>")); 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);
|
||||
}
|
||||
@@ -0,0 +1,316 @@
|
||||
#include "motion.h"
|
||||
#include "hoverboard.h" // hoverTickDelta
|
||||
#include <math.h>
|
||||
|
||||
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 "?";
|
||||
}
|
||||
@@ -0,0 +1,181 @@
|
||||
#include "webui.h"
|
||||
#include "web_page.h"
|
||||
#include "robot_config.h"
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <esp_http_server.h>
|
||||
|
||||
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,<throttle>,<steer>
|
||||
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,<limit>,<response>
|
||||
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,<word>
|
||||
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
|
||||
Submodule
+1
Submodule hoverboard-firmware-hack-FOC added at 4f141cbc97
Reference in New Issue
Block a user