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