First commit
This commit is contained in:
@@ -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 "?";
|
||||
}
|
||||
Reference in New Issue
Block a user