Superminaren/Transpotter-Stuff
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First commit

Browse Source
This commit is contained in:
Superminaren
2026-09-20 18:55:16 +02:00
commit e8d61a1c98
26 changed files with 4351 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/.gitignore
+2
View File
@@ -0,0 +1,2 @@
.pio/
.vscode/
esp32/include/hoverboard.h
+89
View File
@@ -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);
esp32/include/motion.h
+121
View File
@@ -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;
};
esp32/include/robot_config.h
+151
View File
@@ -0,0 +1,151 @@
// Tuning and wiring for the TranspOtter drive controller.
//
// Everything a new chassis needs changing is in this file. The values marked
// MEASURE are geometry — the controller's distance and rotation commands are
// only as accurate as these.
#pragma once
#include <stdint.h>
// ---------------------------------------------------------------- wiring ---
// UART1 to the hoverboard's RIGHT sideboard cable (USART3).
//
// GPIO2/8/9 are strapping pins on the C3 and GPIO8 also drives the Supermini's
// LED, so all three are out. GPIO20 is UART0's default RX, but the GPIO matrix
// routes UART1 here happily — just never call Serial0.begin(), which would
// claim GPIO20/21 and fight for the pin. GPIO21 is deliberately left alone:
// the ROM bootloader logs there at every reset, which would otherwise land in
// the mainboard's receiver.
constexpr int PIN_HOVER_RX = 20; // ESP32 RX <-- mainboard TX (blue, PB10)
constexpr int PIN_HOVER_TX = 10; // ESP32 TX --> mainboard RX (green, PB11)
constexpr uint32_t HOVER_BAUD = 115200; // must match USART3_BAUD in the firmware
// ------------------------------------------------------------- geometry ---
constexpr float WHEEL_DIAMETER_M = 0.1651f; // 6.5" hub motor. MEASURE for other wheels.
constexpr float TRACK_WIDTH_M = 0.570f; // measured, wheel centre to wheel centre
constexpr int TICKS_PER_WHEEL_REV = 90; // 15 pole pairs x 6 hall states (firmware n_polePairs)
constexpr int ODOM_MODULO = 9000; // firmware wraps wheel_cnt at 9000 (= 100 revolutions)
// Set true if a wheel counts or drives backwards. Calibrate once, on blocks,
// with `fwd 1` and `status` — see the README.
constexpr bool INVERT_LEFT = false;
constexpr bool INVERT_RIGHT = false;
constexpr bool INVERT_ODOM_LEFT = false;
constexpr bool INVERT_ODOM_RIGHT = false;
// ------------------------------------------- agreement with the firmware ---
// These mirror Inc/config.h in hoverboard-firmware-hack-FOC. In SPD_MODE a
// command of +/-1000 is a closed-loop request for +/-N_MOT_MAX rpm, so the
// mapping from m/s to command units is exact. Change one side, change both.
constexpr float N_MOT_MAX_RPM = 200.0f; // firmware N_MOT_MAX
constexpr int CMD_MAX = 1000; // firmware INPUT_MAX
// -------------------------------------------------------- motion limits ---
// Body-frame limits. The linear and angular axes are each jerk-limited, so a
// wheel never sees a step in acceleration — this is what keeps a heavy load
// from lurching at the start and nodding at the stop.
constexpr float A_MAX_MPS2 = 0.50f; // linear acceleration
constexpr float J_MAX_MPS3 = 3.00f; // linear jerk: how fast acceleration itself may change.
// 1.0 was too low to stop on a mark: reversing acceleration
// took a full second, and a closed-loop move sailed 160 mm
// past its target. 3.0 still ramps in over ~170 ms, so there
// is no step in acceleration and no lurch.
// Braking may be firmer than acceleration — a gentle start is what stops a
// load lurching, but a lazy stop just feels like slack in the controls.
//
// The multiplier scales the deceleration limit by x and the deceleration JERK
// by x*x. That second part is the one you feel: the delay before braking bites
// is amax/jmax, so squaring the jerk shortens that dead time by a factor of x
// rather than leaving it unchanged.
constexpr float DECEL_BOOST_DEFAULT = 2.0f;
constexpr float W_MAX_RADS = 1.40f; // angular rate at full steer, standstill (~80 deg/s)
constexpr float ALPHA_MAX = 1.60f; // angular acceleration
constexpr float J_MAX_ALPHA = 9.60f; // angular jerk (same reasoning as J_MAX_MPS3)
// The four limits above are the SOFT end of the response range — the tuning
// validated on the bench, and the right feel for hauling a load a long way.
// The values below are the SHARP end, for indoor work where reacting matters
// more than smoothness. The `response` setting (0..1, live-adjustable and
// saved to flash) interpolates between the two, so nothing here is a rebuild.
constexpr float A_MAX_SHARP = 1.50f;
constexpr float J_MAX_SHARP = 9.00f;
constexpr float ALPHA_MAX_SHARP = 3.50f;
constexpr float J_ALPHA_SHARP = 21.0f;
// Startup defaults, overridden by whatever was last saved.
constexpr float RESPONSE_DEFAULT = 0.0f; // soft
constexpr float SPEED_LIMIT_DEFAULT = 0.4f; // 40% of V_MAX on the joystick
// --------------------------------------------------------- steering feel ---
// Angular authority as a function of how fast the robot is already going:
//
// gain(s) = HIGH + (LOW - HIGH) * exp(-s / KNEE), s = |v| / V_MAX
//
// LOW applies at standstill, decaying towards HIGH as speed builds. KNEE is the
// fraction of top speed over which most of the extra authority disappears, so a
// small KNEE means "sharp only when crawling".
constexpr float STEER_GAIN_LOW = 1.00f; // full W_MAX when stationary — spins on the spot
constexpr float STEER_GAIN_HIGH = 0.22f; // at top speed, a gentle curve instead of a swerve
constexpr float STEER_KNEE = 0.15f; // authority is ~halved by 10% of top speed
// ---------------------------------------------------- closed-loop moves ---
constexpr float MOVE_V_MAX = 0.35f; // m/s ceiling during `fwd` (leaves braking margin)
constexpr float MOVE_W_MAX = 0.70f; // rad/s ceiling during `rot`
constexpr float MOVE_TOL_M = 0.010f; // arrival tolerance, metres (~2 hall ticks)
constexpr float MOVE_TOL_RAD = 0.020f; // arrival tolerance, radians (~1.1 deg)
constexpr float MOVE_SETTLE_MPS = 0.02f; // below this a linear move counts as stopped
constexpr float MOVE_SETTLE_RADS = 0.05f; // below this a rotation counts as stopped
// Floor on the approach speed. Without it the braking profile decays towards
// zero as the target nears and the last millimetres take forever. Both values
// are slow enough to stop inside the tolerance above.
constexpr float MOVE_CREEP_MPS = 0.030f;
constexpr float MOVE_CREEP_RADS = 0.050f;
// ---------------------------------------------------------------- timing ---
constexpr uint32_t CONTROL_PERIOD_MS = 10; // 100 Hz command rate (firmware times out at 800 ms)
constexpr uint32_t FEEDBACK_TIMEOUT_MS = 500; // no valid frame for this long -> ramp down
constexpr uint32_t ARM_HOLD_MS = 1500; // hold zero at boot so the firmware arms the motors
// Derived: top wheel speed the firmware will honour, in m/s.
constexpr float PI_F = 3.14159265f;
constexpr float V_MAX = (N_MOT_MAX_RPM / 60.0f) * PI_F * WHEEL_DIAMETER_M;
// Trim gains for closed-loop moves: a straight `fwd` holds heading, a `rot`
// holds position. Both correct the small mismatch between the two wheels.
constexpr float MOVE_HEADING_KP = 1.5f; // rad/s per radian of heading error
constexpr float MOVE_POSITION_KP = 1.0f; // m/s per metre of drift
constexpr float MOVE_TRIM_W_MAX = 0.35f; // clamp on the heading correction
constexpr float MOVE_TRIM_V_MAX = 0.15f; // clamp on the position correction
// Metres of travel per hall tick — the resolution of every closed-loop move.
constexpr float METRES_PER_TICK = (PI_F * WHEEL_DIAMETER_M) / TICKS_PER_WHEEL_REV;
// ------------------------------------------------------------- remote UI ---
// The ESP32 hosts its own WiFi network and serves the control page at
// http://192.168.4.1 — no router and nothing to install. The password is WPA2
// and must be at least 8 characters; an open network would let anyone within
// range drive the machine.
constexpr char AP_SSID[] = "transpotter";
constexpr char AP_PASSWORD[] = "otterdrive";
constexpr int AP_CHANNEL = 6;
constexpr int AP_MAX_CLIENTS = 2;
// The browser sends a drive frame at 10 Hz, so it can afford a much tighter
// deadman than a human typing at the console. Walking out of radio range then
// ramps the machine down in well under a second.
constexpr uint32_t REMOTE_DEADMAN_MS = 400;
// A closed-loop move ends by arriving, so it is exempt from the deadman below.
// That leaves it with no time limit at all, which is dangerous if the odometry
// stops advancing — a dropped link, or a wheel jammed against an obstacle. If
// the remaining distance fails to change by PROGRESS_EPS within STALL_S, the
// move is abandoned.
constexpr float MOVE_PROGRESS_EPS = 0.005f; // metres or radians
constexpr float MOVE_STALL_S = 1.5f;
// A `drive` or `vel` command is a standing order; if nothing refreshes it
// within this long the controller ramps down. Closed-loop moves are exempt —
// they end by arriving.
constexpr uint32_t COMMAND_DEADMAN_MS = 2000;
esp32/include/web_page.h
+217
View File
@@ -0,0 +1,217 @@
// The control page, served from flash. Kept deliberately small and
// dependency-free: no frameworks, no CDN, nothing to fetch. It has to work
// when the phone is joined to the machine's own network with no internet.
#pragma once
static const char PAGE_HTML[] PROGMEM = R"HTML(<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1,user-scalable=no,viewport-fit=cover">
<title>TranspOtter</title>
<style>
:root{
--bg:#0f1214; --panel:#181d21; --line:#2a3238; --text:#e6edf3;
--dim:#8b979f; --accent:#4cc2ff; --ok:#3fb950; --warn:#d29922; --bad:#f85149;
}
*{box-sizing:border-box;-webkit-tap-highlight-color:transparent}
html,body{margin:0;height:100%}
body{
background:var(--bg);color:var(--text);
font:14px/1.4 ui-sans-serif,system-ui,-apple-system,"Segoe UI",Roboto,sans-serif;
display:flex;flex-direction:column;gap:10px;padding:10px;
padding-bottom:calc(10px + env(safe-area-inset-bottom));
overscroll-behavior:none;touch-action:none;
}
header{display:flex;align-items:center;gap:10px;flex-wrap:wrap}
h1{font-size:15px;margin:0;font-weight:600;letter-spacing:.02em}
.pill{
display:inline-flex;align-items:center;gap:6px;background:var(--panel);
border:1px solid var(--line);border-radius:999px;padding:4px 10px;
font-variant-numeric:tabular-nums;font-size:12px;
}
.dot{width:8px;height:8px;border-radius:50%;background:var(--bad)}
.dot.on{background:var(--ok)}
.pad{
flex:1;min-height:210px;background:var(--panel);border:1px solid var(--line);
border-radius:16px;position:relative;overflow:hidden;touch-action:none;
}
.pad canvas{width:100%;height:100%;display:block}
.row{display:flex;gap:8px}
.row>*{flex:1}
button{
background:var(--panel);color:var(--text);border:1px solid var(--line);
border-radius:12px;padding:13px 8px;font-size:14px;font-weight:600;
font-family:inherit;cursor:pointer;
}
button:active{background:#232a30}
button.stop{border-color:#5a4a1f;color:#f0c674}
button.estop{background:#3d1519;border-color:#7d2b31;color:#ff9c96}
.grid{
display:grid;grid-template-columns:repeat(auto-fit,minmax(88px,1fr));gap:6px;
background:var(--panel);border:1px solid var(--line);border-radius:12px;padding:10px;
}
.cell{display:flex;flex-direction:column;gap:2px}
.k{color:var(--dim);font-size:10px;text-transform:uppercase;letter-spacing:.06em}
.val{font-variant-numeric:tabular-nums;font-size:15px;font-weight:600}
.limit{display:flex;align-items:center;gap:10px;background:var(--panel);
border:1px solid var(--line);border-radius:12px;padding:8px 12px}
input[type=range]{flex:1;accent-color:var(--accent)}
@media (min-width:720px){ body{max-width:760px;margin:0 auto} }
</style>
</head>
<body>
<header>
<h1>TranspOtter</h1>
<span class="pill"><span id="dot" class="dot"></span><span id="conn">offline</span></span>
<span class="pill" id="mode">idle</span>
<span class="pill" id="batt">— V</span>
<span class="pill" id="temp">— &deg;C</span>
</header>
<div class="pad" id="pad"><canvas id="cv"></canvas></div>
<div class="limit">
<span class="k">Speed</span>
<input type="range" id="lim" min="5" max="100" value="40">
<span class="val" id="limv" style="min-width:3.2em;text-align:right">40%</span>
</div>
<div class="limit">
<span class="k">Response</span>
<input type="range" id="rsp" min="0" max="100" value="0">
<span class="val" id="rspv" style="min-width:3.2em;text-align:right">soft</span>
</div>
<div class="row">
<button id="p0">HAUL</button>
<button id="p1">NORMAL</button>
<button id="p2">INDOOR</button>
</div>
<div class="row">
<button class="stop" id="bstop">STOP</button>
<button class="estop" id="bestop">E-STOP</button>
</div>
<div class="row">
<button id="barm">ARM</button>
<button id="bzero">ZERO ODO</button>
</div>
<div class="grid">
<div class="cell"><span class="k">speed</span><span class="val" id="v">0.00</span></div>
<div class="cell"><span class="k">turn</span><span class="val" id="w">0.00</span></div>
<div class="cell"><span class="k">steer gain</span><span class="val" id="g">1.00</span></div>
<div class="cell"><span class="k">cmd L/R</span><span class="val" id="c">0/0</span></div>
<div class="cell"><span class="k">distance</span><span class="val" id="d">0.00</span></div>
<div class="cell"><span class="k">heading</span><span class="val" id="h">0.0</span></div>
<div class="cell"><span class="k">frames</span><span class="val" id="f">0</span></div>
<div class="cell"><span class="k">link</span><span class="val" id="lk">down</span></div>
</div>
<script>
const $=id=>document.getElementById(id);
let ws=null, wsOpen=false;
let jx=0, jy=0, limit=0.4, active=false;
// ---- joystick -------------------------------------------------------------
const pad=$('pad'), cv=$('cv'), ctx=cv.getContext('2d');
let W=0,H=0,R=0,cx=0,cy=0,dpr=1;
function size(){
dpr=window.devicePixelRatio||1;
W=pad.clientWidth; H=pad.clientHeight;
cv.width=W*dpr; cv.height=H*dpr; ctx.setTransform(dpr,0,0,dpr,0,0);
cx=W/2; cy=H/2; R=Math.min(W,H)/2-16;
draw();
}
function draw(){
ctx.clearRect(0,0,W,H);
ctx.strokeStyle='#2a3238'; ctx.lineWidth=1;
ctx.beginPath(); ctx.arc(cx,cy,R,0,6.2832); ctx.stroke();
ctx.beginPath(); ctx.moveTo(cx-R,cy); ctx.lineTo(cx+R,cy);
ctx.moveTo(cx,cy-R); ctx.lineTo(cx,cy+R); ctx.stroke();
ctx.strokeStyle='#212930';
ctx.beginPath(); ctx.arc(cx,cy,R*0.5,0,6.2832); ctx.stroke();
const kx=cx+jx*R, ky=cy-jy*R;
ctx.strokeStyle='#4cc2ff'; ctx.lineWidth=2;
ctx.beginPath(); ctx.moveTo(cx,cy); ctx.lineTo(kx,ky); ctx.stroke();
ctx.fillStyle=active?'#4cc2ff':'#5a666e';
ctx.beginPath(); ctx.arc(kx,ky,20,0,6.2832); ctx.fill();
}
function at(e){
const r=pad.getBoundingClientRect();
let x=(e.clientX-r.left-cx)/R, y=-(e.clientY-r.top-cy)/R;
const m=Math.hypot(x,y); if(m>1){x/=m;y/=m;}
jx=x; jy=y; draw();
}
pad.addEventListener('pointerdown',e=>{active=true;pad.setPointerCapture(e.pointerId);at(e);});
pad.addEventListener('pointermove',e=>{if(active)at(e);});
function release(){active=false;jx=0;jy=0;draw();}
pad.addEventListener('pointerup',release);
pad.addEventListener('pointercancel',release);
window.addEventListener('resize',size);
// Speed scales top end; response scales how hard it accelerates to get there.
// Both live on the device and are saved there, so a fresh phone picks up the
// settings you last used rather than imposing its own.
let response=0, synced=false;
function rspLabel(v){ return v<15?'soft':v<40?'easy':v<70?'brisk':v<90?'sharp':'max'; }
function pushSettings(){ send('s,'+limit.toFixed(2)+','+response.toFixed(2)); }
$('lim').addEventListener('input',e=>{
limit=e.target.value/100; $('limv').textContent=e.target.value+'%'; pushSettings();
});
$('rsp').addEventListener('input',e=>{
response=e.target.value/100; $('rspv').textContent=rspLabel(+e.target.value); pushSettings();
});
function preset(v){
response=v/100; $('rsp').value=v; $('rspv').textContent=rspLabel(v); pushSettings();
}
$('p0').onclick=()=>preset(0);
$('p1').onclick=()=>preset(45);
$('p2').onclick=()=>preset(100);
// ---- link -----------------------------------------------------------------
function connect(){
ws=new WebSocket('ws://'+location.host+'/ws');
ws.onopen =()=>{wsOpen=true;synced=false;$('dot').classList.add('on');$('conn').textContent='online';};
ws.onclose=()=>{wsOpen=false;$('dot').classList.remove('on');$('conn').textContent='offline';
setTimeout(connect,700);};
ws.onerror=()=>ws.close();
ws.onmessage=e=>{
let t; try{t=JSON.parse(e.data);}catch(_){return;}
$('mode').textContent=t.m;
$('batt').textContent=t.b.toFixed(1)+' V';
$('temp').textContent=t.t.toFixed(0)+' °C';
$('v').textContent=t.v.toFixed(2);
$('w').textContent=t.w.toFixed(2);
$('g').textContent=t.g.toFixed(2);
$('c').textContent=t.cl+'/'+t.cr;
$('d').textContent=t.d.toFixed(2);
$('h').textContent=t.h.toFixed(1);
$('f').textContent=t.fo+(t.fb?(' / '+t.fb+' bad'):'');
$('lk').textContent=t.k?'up':'down';
$('lk').style.color=t.k?'var(--ok)':'var(--bad)';
if(!synced && t.lim!==undefined){ // adopt the device's saved values once
synced=true;
limit=t.lim; $('lim').value=Math.round(t.lim*100); $('limv').textContent=Math.round(t.lim*100)+'%';
response=t.rsp; $('rsp').value=Math.round(t.rsp*100); $('rspv').textContent=rspLabel(Math.round(t.rsp*100));
}
};
}
function send(s){ if(wsOpen) try{ws.send(s);}catch(_){ } }
function cmd(w){ send('c,'+w); }
$('bstop').onclick =()=>{release();cmd('stop');};
$('bestop').onclick=()=>{release();cmd('estop');};
$('barm').onclick =()=>cmd('arm');
$('bzero').onclick =()=>cmd('zero');
// The drive frame is also the heartbeat: it goes out at 10 Hz whether or not
// the stick moved. Stop sending — background the tab, walk out of range — and
// the firmware's deadman ramps the machine down.
setInterval(()=>send('d,'+jy.toFixed(3)+','+jx.toFixed(3)),100);
size(); connect();
</script>
</body>
</html>)HTML";
esp32/include/webui.h
+56
View File
@@ -0,0 +1,56 @@
// WiFi access point + web UI for hand-driving the machine.
//
// The ESP32 hosts its own network; a browser at http://192.168.4.1 gets a
// touch joystick and live telemetry over a WebSocket. The page sends a drive
// frame at 10 Hz and the server answers each one with telemetry, so the
// request/response pairing doubles as the heartbeat — stop sending and the
// deadman ramps the machine down.
//
// The HTTP server runs in its own FreeRTOS task, so everything shared with the
// control loop goes through a spinlock.
#pragma once
#include <Arduino.h>
#include "robot_config.h"
namespace webui {
// Snapshot the control loop publishes for the browser to read.
struct Telemetry {
const char* mode = "idle";
int16_t cmdL = 0, cmdR = 0;
float v = 0, w = 0, gain = 1;
float dist = 0, headDeg = 0;
float battery = 0, temp = 0;
bool link = false, armed = false;
uint32_t framesOk = 0, framesBad = 0;
float limit = SPEED_LIMIT_DEFAULT;
float response = RESPONSE_DEFAULT;
};
struct DriveInput {
float throttle = 0, steer = 0;
};
struct Settings {
float limit = SPEED_LIMIT_DEFAULT; // joystick throttle scale, 0..1
float response = RESPONSE_DEFAULT; // acceleration sharpness, 0..1
};
void begin();
// Called every control tick.
void publish(const Telemetry& t);
// True if a drive frame arrived since the last call.
bool takeDrive(DriveInput& out);
// True if a one-shot command word arrived ("stop", "estop", "arm", "zero").
bool takeCommand(char* buf, size_t len);
// True if the browser changed the tuning sliders.
bool takeSettings(Settings& out);
uint8_t clients();
} // namespace webui
esp32/platformio.ini
+34
View File
@@ -0,0 +1,34 @@
; TranspOtter — ESP32-C3 Supermini motion controller for a hoverboard mainboard
; running hoverboard-firmware-hack-FOC (VARIANT_USART + TANK_STEERING + SPD_MODE).
;
; pio run build
; pio run -t upload flash over USB-C
; pio device monitor open the console (115200)
[env:supermini]
platform = espressif32
board = esp32-c3-devkitm-1
framework = arduino
monitor_speed = 115200
monitor_filters = send_on_enter, colorize
; The C3's native USB-Serial-JTAG is not a real UART, so a baud change is both
; pointless and harmful — esptool's stub loses the link mid-handshake. 115200 is
; esptool's default, so it skips the renegotiation entirely.
upload_speed = 115200
; Reset over USB rather than toggling DTR/RTS like a UART bridge. Without this,
; a half-finished upload leaves the chip desynced and every later attempt hangs
; in esptool's `Connecting....` until it is physically unplugged.
board_upload.before_reset = usb_reset
; Splices --no-stub into the esptool invocation; see the script for why.
extra_scripts = post:scripts/no_stub.py
build_flags =
; The Supermini's USB-C is the C3's native USB, so the console is USB CDC
; and UART1 stays free for the hoverboard link.
-DARDUINO_USB_MODE=1
-DARDUINO_USB_CDC_ON_BOOT=1
-Wall
-Wextra
esp32/scripts/no_stub.py
+18
View File
@@ -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")
esp32/src/hoverboard.cpp
+114
View File
@@ -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;
}
esp32/src/main.cpp
+347
View File
@@ -0,0 +1,347 @@
// TranspOtter — ESP32-C3 Supermini motion controller.
//
// Talks to a hoverboard mainboard running hoverboard-firmware-hack-FOC
// (VARIANT_USART + TANK_STEERING + SPD_MODE) at 100 Hz, and exposes a line
// oriented console on USB for driving and for scripted moves.
//
// Type `help` in the monitor for the command list.
#include <Arduino.h>
#include "robot_config.h"
#include "hoverboard.h"
#include "motion.h"
#include "webui.h"
#include <Preferences.h>
namespace {
HoverboardLink hoverLink; // not `link`: POSIX link() from <unistd.h> makes that ambiguous
Odometry odo;
Motion motion;
uint32_t bootMs = 0;
uint32_t lastCmdMs = 0;
uint32_t deadmanMs = COMMAND_DEADMAN_MS; // tightened while the web UI drives
Preferences prefs;
float speedLimit = SPEED_LIMIT_DEFAULT; // joystick throttle scale
bool settingsDirty = false;
uint32_t settingsDirtyMs = 0;
bool armed = false;
bool linkWasUp = false;
bool telemetry = false;
uint32_t lastTelemMs = 0;
String lineBuf;
SemaphoreHandle_t controlMux = nullptr;
// ------------------------------------------------------------- console ---
void printHelp() {
Serial.println();
Serial.println(F("TranspOtter console"));
Serial.println(F(" drive <throttle> <steer> -1..1 each; steer is speed-curved"));
Serial.println(F(" vel <m/s> <rad/s> physical units, no steering curve"));
Serial.println(F(" fwd <metres> closed-loop straight move (holds heading)"));
Serial.println(F(" rot <degrees> closed-loop spin on the spot (+ = left)"));
Serial.println(F(" stop ramp down through the jerk limits"));
Serial.println(F(" estop cut the command now, then latch"));
Serial.println(F(" arm clear an e-stop"));
Serial.println(F(" zero reset the odometry accumulators"));
Serial.println(F(" speed <0..1> joystick throttle scale (saved)"));
Serial.println(F(" resp <0..1> acceleration sharpness: 0 soft, 1 sharp (saved)"));
Serial.println(F(" decel <1..6> how much firmer braking is than acceleration (saved)"));
Serial.println(F(" tel [on|off] stream telemetry at 5 Hz"));
Serial.println(F(" status one-shot state dump"));
Serial.println();
}
void printStatus() {
const HoverFeedback& fb = hoverLink.feedback();
Serial.printf("mode=%-8s armed=%d link=%d cmd=%+5d/%+5d v=%+.3f m/s w=%+.3f rad/s gain=%.2f\r\n",
motion.modeName(), armed ? 1 : 0, hoverLink.linkUp() ? 1 : 0,
motion.cmdLeft(), motion.cmdRight(),
motion.linear(), motion.angular(), motion.steerGain());
Serial.printf("odo: L=%+.3f m R=%+.3f m dist=%+.3f m head=%+.1f deg\r\n",
odo.left(), odo.right(), odo.distance(), odo.heading() * 180.0 / PI_F);
Serial.printf("tuning: speed %.0f%% response %.2f decel %.2fx\r\n",
speedLimit * 100.0f, motion.response(), motion.decelBoost());
Serial.printf("board: %.2f V %.1f C rpm L=%d R=%d frames ok=%lu bad=%lu rx=%lu bytes\r\n",
hoverLink.batteryVolts(), hoverLink.boardTempC(),
fb.speedL_meas, fb.speedR_meas,
(unsigned long)hoverLink.framesOk(), (unsigned long)hoverLink.framesBad(),
(unsigned long)hoverLink.rxBytes());
if (motion.busy()) {
Serial.printf("move: %.3f remaining\r\n", motion.remaining());
}
}
void saveSettings() {
prefs.putFloat("limit", speedLimit);
prefs.putFloat("response", motion.response());
prefs.putFloat("decel", motion.decelBoost());
Serial.printf("saved: speed %.0f%% response %.2f\r\n", speedLimit * 100.0f, motion.response());
}
// Dragging a slider fires continuously, so settle first and write once — NVS
// has a finite number of erase cycles and there is no hurry.
void markSettingsDirty() {
settingsDirty = true;
settingsDirtyMs = millis();
}
bool applyWord(const char* w) {
if (!strcmp(w, "stop")) {
motion.stop();
Serial.println(F("stopping"));
} else if (!strcmp(w, "estop")) {
motion.emergencyStop();
Serial.println(F("E-STOP latched — send `arm` to clear"));
} else if (!strcmp(w, "arm")) {
motion.clearEStop();
Serial.println(F("cleared"));
} else if (!strcmp(w, "zero")) {
odo.reset();
Serial.println(F("odometry zeroed"));
} else {
return false;
}
return true;
}
void handleLine(String line) {
line.trim();
if (line.isEmpty()) return;
// Split into a verb and up to two numeric arguments.
char verb[16] = {0};
char word[16] = {0};
float a1 = 0, a2 = 0;
const int n = sscanf(line.c_str(), "%15s %f %f", verb, &a1, &a2);
const int nWord = sscanf(line.c_str(), "%15s %15s", verb, word);
if (n < 1 && nWord < 1) return;
const String cmd(verb);
if (cmd == "help" || cmd == "?") {
printHelp();
} else if (cmd == "status" || cmd == "st") {
printStatus();
} else if (cmd == "drive") {
if (n < 3) { Serial.println(F("usage: drive <throttle -1..1> <steer -1..1>")); return; }
motion.setDrive(a1, a2);
lastCmdMs = millis();
deadmanMs = COMMAND_DEADMAN_MS;
Serial.printf("drive %.2f %.2f\r\n", a1, a2);
} else if (cmd == "vel") {
if (n < 3) { Serial.println(F("usage: vel <m/s> <rad/s>")); return; }
motion.setVelocity(a1, a2);
lastCmdMs = millis();
deadmanMs = COMMAND_DEADMAN_MS;
Serial.printf("vel %.3f %.3f\r\n", a1, a2);
} else if (cmd == "fwd") {
if (n < 2) { Serial.println(F("usage: fwd <metres>")); return; }
if (!hoverLink.linkUp()) { Serial.println(F("refused: no odometry link")); return; }
motion.moveDistance(a1, odo);
Serial.printf("fwd %.3f m\r\n", a1);
} else if (cmd == "rot") {
if (n < 2) { Serial.println(F("usage: rot <degrees>")); return; }
if (!hoverLink.linkUp()) { Serial.println(F("refused: no odometry link")); return; }
motion.rotate(a1 * PI_F / 180.0f, odo);
Serial.printf("rot %.1f deg\r\n", a1);
} else if (applyWord(verb)) {
// stop / estop / arm / zero, handled in common with the web UI
} else if (cmd == "speed") {
if (n < 2) { Serial.printf("speed %.0f%%\r\n", speedLimit * 100.0f); return; }
speedLimit = constrain(a1, 0.05f, 1.0f);
markSettingsDirty();
Serial.printf("speed %.0f%%\r\n", speedLimit * 100.0f);
} else if (cmd == "resp") {
if (n < 2) { Serial.printf("response %.2f\r\n", motion.response()); return; }
motion.setResponse(a1);
markSettingsDirty();
Serial.printf("response %.2f\r\n", motion.response());
} else if (cmd == "decel") {
if (n < 2) { Serial.printf("decel %.2fx\r\n", motion.decelBoost()); return; }
motion.setDecelBoost(a1);
markSettingsDirty();
Serial.printf("decel %.2fx\r\n", motion.decelBoost());
} else if (cmd == "tel") {
if (nWord < 2) telemetry = !telemetry;
else if (!strcmp(word, "on") || !strcmp(word, "1")) telemetry = true;
else if (!strcmp(word, "off") || !strcmp(word, "0")) telemetry = false;
else telemetry = !telemetry;
Serial.printf("telemetry %s\r\n", telemetry ? "on" : "off");
} else {
Serial.printf("unknown command: %s (try `help`)\r\n", verb);
}
}
void pumpConsole() {
while (Serial.available() > 0) {
const char c = (char)Serial.read();
if (c == '\n' || c == '\r') {
if (lineBuf.length() > 0) {
handleLine(lineBuf);
lineBuf = "";
}
} else if (lineBuf.length() < 96) {
lineBuf += c;
}
}
}
// ----------------------------------------------------------- control ---
// Everything with a deadline. Runs above the HTTP server (priority 4) and the
// Arduino loop task (priority 1): the mainboard cuts the motors if a command
// frame is over 0.8 s late, and neither a WiFi burst nor a Serial write that
// blocks on a slow terminal may be allowed to cause that.
void controlTick(uint32_t now, float dt) {
if (hoverLink.poll()) {
const HoverFeedback& fb = hoverLink.feedback();
odo.update(fb.wheelL_cnt, fb.wheelR_cnt);
}
// ---- web UI input ----
webui::DriveInput remote;
if (webui::takeDrive(remote)) {
// The speed scale is applied here rather than in the browser, so a stale
// or misbehaving page cannot ask for more than the saved limit.
motion.setDrive(remote.throttle * speedLimit, remote.steer);
lastCmdMs = now;
deadmanMs = REMOTE_DEADMAN_MS;
}
webui::Settings st;
if (webui::takeSettings(st)) {
speedLimit = st.limit;
motion.setResponse(st.response);
markSettingsDirty();
}
if (settingsDirty && (now - settingsDirtyMs) > 1500) {
settingsDirty = false;
saveSettings();
}
char word[16];
if (webui::takeCommand(word, sizeof(word))) {
applyWord(word);
}
if (!armed && (now - bootMs) >= ARM_HOLD_MS) {
armed = true;
Serial.println(F("armed — type `help` for commands"));
}
// Losing feedback means we are driving blind: wind the command down rather
// than waiting for the mainboard's own 0.8 s timeout to cut the motors dead.
const bool up = hoverLink.linkUp();
if (linkWasUp && !up) {
motion.stop();
Serial.println(F("! feedback lost — ramping down"));
} else if (!linkWasUp && up) {
Serial.println(F("feedback link up"));
}
linkWasUp = up;
// Deadman on standing velocity orders. Closed-loop moves end by arriving, so
// they are left alone.
if ((motion.mode() == Motion::Mode::Drive || motion.mode() == Motion::Mode::Velocity) &&
(now - lastCmdMs) > deadmanMs) {
motion.stop();
Serial.println(F("! command timeout — ramping down"));
}
motion.update(odo, dt);
if (motion.takeAbort()) {
Serial.println(F("! move abandoned — no odometry progress"));
}
{
const HoverFeedback& fb = hoverLink.feedback();
webui::Telemetry t;
t.mode = motion.modeName();
t.cmdL = motion.cmdLeft();
t.cmdR = motion.cmdRight();
t.v = motion.linear();
t.w = motion.angular();
t.gain = motion.steerGain();
t.dist = (float)odo.distance();
t.headDeg = (float)(odo.heading() * 180.0 / PI_F);
t.battery = hoverLink.batteryVolts();
t.temp = hoverLink.boardTempC();
t.link = up;
t.armed = armed;
t.framesOk = hoverLink.framesOk();
t.framesBad = hoverLink.framesBad();
t.limit = speedLimit;
t.response = motion.response();
(void)fb;
webui::publish(t);
}
if (armed) {
hoverLink.send(motion.cmdLeft(), motion.cmdRight());
} else {
hoverLink.send(0, 0);
}
}
void controlTask(void*) {
TickType_t last = xTaskGetTickCount();
uint32_t prev = millis();
for (;;) {
vTaskDelayUntil(&last, pdMS_TO_TICKS(CONTROL_PERIOD_MS));
const uint32_t now = millis();
const float dt = (now - prev) / 1000.0f;
prev = now;
if (xSemaphoreTake(controlMux, pdMS_TO_TICKS(50)) == pdTRUE) {
controlTick(now, dt > 0.0f ? dt : 0.001f);
xSemaphoreGive(controlMux);
}
}
}
} // namespace
void setup() {
Serial.begin(115200);
hoverLink.begin(Serial1);
prefs.begin("transpotter", false);
speedLimit = prefs.getFloat("limit", SPEED_LIMIT_DEFAULT);
motion.setDecelBoost(prefs.getFloat("decel", DECEL_BOOST_DEFAULT));
motion.setResponse(prefs.getFloat("response", RESPONSE_DEFAULT));
webui::begin();
// Measured from here, not from power-on: bringing up WiFi takes a moment and
// the arming hold should cover the window in which we are actually sending.
bootMs = millis();
lastCmdMs = bootMs;
// The firmware only arms its motors once it has seen both inputs sit near
// zero, so hold a zero command over the arming window before doing anything.
Serial.println();
controlMux = xSemaphoreCreateMutex();
xTaskCreate(controlTask, "control", 4096, nullptr, 5, nullptr);
Serial.println(F("TranspOtter controller starting — holding zero to arm the mainboard"));
}
void loop() {
// Console and logging only; nothing here can delay a command frame.
pumpConsole();
const uint32_t now = millis();
if (telemetry && (now - lastTelemMs) >= 200) {
lastTelemMs = now;
Serial.printf("t=%lu cmd=%+5d/%+5d v=%+.3f w=%+.3f dist=%+.3f head=%+.1f bat=%.2f\r\n",
(unsigned long)now, motion.cmdLeft(), motion.cmdRight(),
motion.linear(), motion.angular(),
odo.distance(), odo.heading() * 180.0 / PI_F,
hoverLink.batteryVolts());
}
delay(2);
}
esp32/src/motion.cpp
+316
View File
@@ -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 "?";
}
esp32/src/webui.cpp
+181
View File
@@ -0,0 +1,181 @@
#include "webui.h"
#include "web_page.h"
#include "robot_config.h"
#include <WiFi.h>
#include <esp_http_server.h>
namespace webui {
namespace {
httpd_handle_t server = nullptr;
// Everything below is touched by both the HTTP task and the control loop.
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
Telemetry shared{};
DriveInput pendingDrive{};
bool haveDrive = false;
char pendingCmd[16] = {0};
bool haveCmd = false;
Settings pendingSettings{};
bool haveSettings = false;
float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
esp_err_t indexHandler(httpd_req_t* req) {
httpd_resp_set_type(req, "text/html");
httpd_resp_set_hdr(req, "Cache-Control", "no-store");
return httpd_resp_send(req, PAGE_HTML, HTTPD_RESP_USE_STRLEN);
}
int buildTelemetry(char* out, size_t len) {
Telemetry t;
portENTER_CRITICAL(&mux);
t = shared;
portEXIT_CRITICAL(&mux);
return snprintf(out, len,
"{\"m\":\"%s\",\"a\":%d,\"k\":%d,\"cl\":%d,\"cr\":%d,"
"\"v\":%.3f,\"w\":%.3f,\"g\":%.2f,\"d\":%.3f,\"h\":%.1f,"
"\"b\":%.2f,\"t\":%.1f,\"fo\":%lu,\"fb\":%lu,"
"\"lim\":%.2f,\"rsp\":%.2f}",
t.mode, t.armed ? 1 : 0, t.link ? 1 : 0, t.cmdL, t.cmdR,
t.v, t.w, t.gain, t.dist, t.headDeg,
t.battery, t.temp,
(unsigned long)t.framesOk, (unsigned long)t.framesBad,
t.limit, t.response);
}
esp_err_t wsHandler(httpd_req_t* req) {
if (req->method == HTTP_GET) return ESP_OK; // handshake only
httpd_ws_frame_t in{};
in.type = HTTPD_WS_TYPE_TEXT;
// First call with len 0 reports the frame size.
esp_err_t err = httpd_ws_recv_frame(req, &in, 0);
if (err != ESP_OK) return err;
if (in.len == 0 || in.len > 64) return ESP_OK;
uint8_t buf[65] = {0};
in.payload = buf;
err = httpd_ws_recv_frame(req, &in, in.len);
if (err != ESP_OK) return err;
if (buf[0] == 'd') { // d,<throttle>,<steer>
float th = 0, st = 0;
if (sscanf((const char*)buf, "d,%f,%f", &th, &st) == 2) {
portENTER_CRITICAL(&mux);
pendingDrive.throttle = clampf(th, -1.0f, 1.0f);
pendingDrive.steer = clampf(st, -1.0f, 1.0f);
haveDrive = true;
portEXIT_CRITICAL(&mux);
}
} else if (buf[0] == 's') { // s,<limit>,<response>
float lim = 0, rsp = 0;
if (sscanf((const char*)buf, "s,%f,%f", &lim, &rsp) == 2) {
portENTER_CRITICAL(&mux);
pendingSettings.limit = clampf(lim, 0.05f, 1.0f);
pendingSettings.response = clampf(rsp, 0.0f, 1.0f);
haveSettings = true;
portEXIT_CRITICAL(&mux);
}
} else if (buf[0] == 'c') { // c,<word>
char word[16] = {0};
if (sscanf((const char*)buf, "c,%15s", word) == 1) {
portENTER_CRITICAL(&mux);
strncpy(pendingCmd, word, sizeof(pendingCmd) - 1);
pendingCmd[sizeof(pendingCmd) - 1] = '\0';
haveCmd = true;
portEXIT_CRITICAL(&mux);
}
}
// Answering every frame keeps the browser's 10 Hz cadence as the heartbeat
// and avoids the bookkeeping of async sends to tracked sockets.
char json[320];
const int n = buildTelemetry(json, sizeof(json));
if (n <= 0) return ESP_OK;
httpd_ws_frame_t out{};
out.type = HTTPD_WS_TYPE_TEXT;
out.payload = (uint8_t*)json;
out.len = (size_t)n;
return httpd_ws_send_frame(req, &out);
}
} // namespace
void begin() {
WiFi.mode(WIFI_AP);
WiFi.softAP(AP_SSID, AP_PASSWORD, AP_CHANNEL, false, AP_MAX_CLIENTS);
httpd_config_t cfg = HTTPD_DEFAULT_CONFIG();
// Below the Arduino loop task would starve the 100 Hz control loop under
// load; above it, a burst of HTTP work could delay a command frame. The
// mainboard closes its own loops at 16 kHz, so a late setpoint is harmless,
// but there is no reason to make it likely.
cfg.task_priority = 4;
cfg.stack_size = 4096;
cfg.max_open_sockets = 4;
cfg.lru_purge_enable = true;
if (httpd_start(&server, &cfg) != ESP_OK) {
Serial.println(F("! web UI: httpd_start failed"));
server = nullptr;
return;
}
httpd_uri_t page{};
page.uri = "/";
page.method = HTTP_GET;
page.handler = indexHandler;
page.user_ctx = nullptr;
httpd_register_uri_handler(server, &page);
httpd_uri_t ws{};
ws.uri = "/ws";
ws.method = HTTP_GET;
ws.handler = wsHandler;
ws.user_ctx = nullptr;
ws.is_websocket = true;
httpd_register_uri_handler(server, &ws);
Serial.printf("web UI: join WiFi \"%s\" (password %s), then open http://%s\r\n",
AP_SSID, AP_PASSWORD, WiFi.softAPIP().toString().c_str());
}
void publish(const Telemetry& t) {
portENTER_CRITICAL(&mux);
shared = t;
portEXIT_CRITICAL(&mux);
}
bool takeDrive(DriveInput& out) {
bool got = false;
portENTER_CRITICAL(&mux);
if (haveDrive) { out = pendingDrive; haveDrive = false; got = true; }
portEXIT_CRITICAL(&mux);
return got;
}
bool takeCommand(char* buf, size_t len) {
bool got = false;
portENTER_CRITICAL(&mux);
if (haveCmd) { strncpy(buf, pendingCmd, len - 1); buf[len - 1] = '\0'; haveCmd = false; got = true; }
portEXIT_CRITICAL(&mux);
return got;
}
bool takeSettings(Settings& out) {
bool got = false;
portENTER_CRITICAL(&mux);
if (haveSettings) { out = pendingSettings; haveSettings = false; got = true; }
portEXIT_CRITICAL(&mux);
return got;
}
uint8_t clients() { return WiFi.softAPgetStationNum(); }
} // namespace webui