Draft: shared protocol, remote firmware, project docs
Not yet verified on hardware. - shared/HexapodProtocol: CmdPacket (now 20 bytes, adds remote_roll / remote_pitch), FLAG_* bits and radio settings as the single protocol definition; static payload size = sizeof(CmdPacket) on both radios. - robot: use the shared protocol; vx = forward (harness w/x <-> q/e); InputMapper adds remote tilt on top of the right stick when FLAG_REMOTE_GYRO is set, and robot.ino no longer zeroes stick roll/pitch when it's off. - remote (Teensy 4.1): Sticks (calibration, EEPROM), RemoteIMU, RemoteState (interlocks, link-loss disarm, smoothing), PacketBuilder, RadioLink, NextionUI, SerialConsole. - CLAUDE.md, README.md, remote/README.md, remote/NEXTION.md. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
ad77b06c36
commit
91dac16fca
30 files changed
+1706
-57
No files matched your search
@@ -0,0 +1,110 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// =====================================================================
|
||||
// Remote handset config (Teensy 4.1). Everything here is a placeholder
|
||||
// until checked against your real wiring and parts - see the BENCH notes.
|
||||
// Radio address/channel/rate/PA/payload size are NOT here: they live in
|
||||
// shared/HexapodProtocol so they can't drift from the robot.
|
||||
// =====================================================================
|
||||
|
||||
// ---- Loop rate ----
|
||||
// Must be >= the robot's CONTROL_HZ (60) so every robot control tick sees a
|
||||
// fresh packet. 100 Hz gives margin for the odd dropped packet.
|
||||
constexpr float TX_HZ = 100.0f;
|
||||
|
||||
// ---- Radio (nRF24L01) on SPI0: MOSI 11, MISO 12, SCK 13 ----
|
||||
// BENCH: placeholder CE/CSN pins - match your wiring.
|
||||
constexpr uint8_t RADIO_CE_PIN = 9;
|
||||
constexpr uint8_t RADIO_CSN_PIN = 10;
|
||||
// Auto-retransmit: (delay units of 250us, count). Kept short on purpose - a
|
||||
// newer packet replaces a lost one 10 ms later anyway, and write() blocks for
|
||||
// the whole retry sequence. Worst case here is ~4 attempts x ~1.7 ms.
|
||||
constexpr uint8_t RADIO_RETRY_DELAY = 1;
|
||||
constexpr uint8_t RADIO_RETRY_COUNT = 3;
|
||||
// No acks for this long -> remote considers the link lost and disarms itself,
|
||||
// mirroring the robot's LINK_TIMEOUT_SEC (0.3 s) failsafe, so the robot can't
|
||||
// spring back up on its own when the link returns.
|
||||
constexpr uint32_t LINK_LOST_MS = 300;
|
||||
|
||||
// ---- Joysticks: 2 x 3-axis analog potentiometer sticks ----
|
||||
// Logical axis order used everywhere in the remote code.
|
||||
enum StickAxis : uint8_t { L_X = 0, L_Y, L_TWIST, R_X, R_Y, R_TWIST, NUM_AXES };
|
||||
|
||||
// BENCH: placeholder pins. A4/A5 (18/19) are skipped because they're the I2C
|
||||
// pins the MPU6050 uses.
|
||||
constexpr uint8_t STICK_PINS[NUM_AXES] = { A0, A1, A2, A3, A6, A7 };
|
||||
|
||||
// Each logical axis must end up: X + = pushed RIGHT, Y + = pushed FORWARD
|
||||
// (away from you), TWIST + = rotated COUNTERCLOCKWISE seen from above.
|
||||
// BENCH: flip any axis that reads the wrong way on the serial 'p' status dump.
|
||||
constexpr bool STICK_INVERT[NUM_AXES] = { false, false, false, false, false, false };
|
||||
|
||||
constexpr int ADC_BITS = 12; // 0..4095
|
||||
constexpr int ADC_AVERAGING = 8; // hardware averaging per analogRead()
|
||||
|
||||
// Fallback calibration used until you run the on-device sweep calibration
|
||||
// (which is saved to EEPROM and overrides these).
|
||||
// BENCH: typical pot sticks don't use the full electrical range - run the
|
||||
// sweep calibration once, then optionally paste the printed values here.
|
||||
constexpr int16_t STICK_DEFAULT_MIN = 200;
|
||||
constexpr int16_t STICK_DEFAULT_CENTER = 2048;
|
||||
constexpr int16_t STICK_DEFAULT_MAX = 3900;
|
||||
|
||||
// At power-on the resting position of each axis is re-captured as its center
|
||||
// (pots drift a little with temperature). Rejected if a stick is visibly
|
||||
// deflected, i.e. further than this from the stored center (ADC counts).
|
||||
constexpr int16_t CENTER_CAPTURE_TOLERANCE = 250;
|
||||
// A sweep calibration is rejected unless every axis travels at least this
|
||||
// far on both sides of center (ADC counts) - catches "forgot to move a stick".
|
||||
constexpr int16_t CAL_MIN_HALF_SPAN = 600;
|
||||
|
||||
// Deadzone as a fraction of full deflection; output is rescaled so motion
|
||||
// starts smoothly from 0 at the deadzone edge instead of jumping.
|
||||
constexpr float STICK_DEADZONE[NUM_AXES] = { 0.06f, 0.06f, 0.10f, 0.06f, 0.06f, 0.10f };
|
||||
// 0 = linear, 1 = fully cubic. A bit of expo gives fine control near center.
|
||||
constexpr float STICK_EXPO[NUM_AXES] = { 0.25f, 0.25f, 0.30f, 0.20f, 0.20f, 0.20f };
|
||||
// One-pole low-pass on raw ADC counts (1 = no filtering). Pots are noisy.
|
||||
constexpr float STICK_FILTER_ALPHA = 0.5f;
|
||||
|
||||
// ---- Remote IMU: MPU6050 on I2C (Wire: SDA 18, SCL 19) ----
|
||||
constexpr uint8_t IMU_ADDR = 0x68; // 0x69 if AD0 is pulled high
|
||||
// Mounting remap: which chip axis (0=X,1=Y,2=Z) and sign gives each handset
|
||||
// axis. Handset frame: +X = away from you, +Y = your left, +Z = up.
|
||||
// BENCH: with the defaults the chip is assumed flat, component side up, its
|
||||
// X arrow pointing away from you. Verify with the serial 'p' dump: tipping
|
||||
// the far edge down must read +pitch, dipping the right side must read +roll.
|
||||
constexpr uint8_t IMU_AXIS_FROM_CHIP[3] = { 0, 1, 2 };
|
||||
constexpr int8_t IMU_AXIS_SIGN[3] = { +1, +1, +1 };
|
||||
constexpr float IMU_ALPHA = 0.98f; // complementary filter, same as the robot
|
||||
constexpr int IMU_GYRO_BIAS_SAMPLES = 200; // at boot; hold the remote still
|
||||
|
||||
// Handset tilt -> CmdPacket.remote_roll/pitch. This many degrees of tilt away
|
||||
// from the zeroed "level" pose = full scale (1000); the robot then scales
|
||||
// full scale to its own MAX_TILT_DEG.
|
||||
constexpr float REMOTE_TILT_FULL_SCALE_DEG = 30.0f;
|
||||
constexpr float REMOTE_TILT_DEADZONE_DEG = 2.0f; // ignore hand tremor around level
|
||||
// Toggling "remote gyro" fades the tilt in/out at this rate (fraction/s)
|
||||
// instead of stepping it. The robot rate-limits pose as well; this just
|
||||
// keeps the flag change itself from being a hard edge.
|
||||
constexpr float REMOTE_TILT_FADE_PER_SEC = 2.0f;
|
||||
|
||||
// ---- Operator setpoints ----
|
||||
constexpr uint8_t SPEED_SCALE_DEFAULT = 128;
|
||||
constexpr float SPEED_SCALE_RATE_PER_SEC = 255.0f; // slider jumps ramp over <= 1 s (robot doesn't smooth twist)
|
||||
constexpr uint8_t SPEED_SCALE_STEP = 25; // serial console +/- step
|
||||
// FLAG_WALK stays set this long after the left stick returns to center, so
|
||||
// hovering at the deadzone edge doesn't chatter STAND <-> WALKING.
|
||||
constexpr uint32_t WALK_LINGER_MS = 250;
|
||||
// Arming is refused unless every stick axis is within this of center
|
||||
// (post-deadzone, -1000..1000 scale), so the robot never arms mid-command.
|
||||
constexpr int16_t ARM_CENTER_TOLERANCE = 50;
|
||||
|
||||
// ---- Nextion touchscreen on Serial2 (RX2 = pin 7, TX2 = pin 8) ----
|
||||
#define NEXTION_SERIAL Serial2
|
||||
// BENCH: Nextion ships at 9600. The HMI's Program.s must set `bauds=115200`
|
||||
// (see remote/NEXTION.md) or change this to match.
|
||||
constexpr uint32_t NEXTION_BAUD = 115200;
|
||||
constexpr uint32_t NEXTION_FULL_REFRESH_MS = 2000; // re-push everything periodically (self-heals after a display reset)
|
||||
constexpr uint32_t NEXTION_LINK_REFRESH_MS = 250; // link-quality text update rate
|
||||
constexpr uint32_t STATUS_MESSAGE_MS = 2500; // how long transient status messages stay up
|
||||
@@ -0,0 +1,75 @@
|
||||
# Nextion HMI build guide
|
||||
|
||||
The firmware drives the display by **component name**, and the display
|
||||
talks back with small custom frames sent from each button's touch event.
|
||||
Build one page in Nextion Editor to match the tables below. Layout, fonts
|
||||
and colors are up to you. The names and event code must match exactly.
|
||||
|
||||
The remote is authoritative. Use **plain Button** components (not
|
||||
Dual-state buttons) and don't toggle anything locally in the HMI. The
|
||||
remote repaints every label and color from its own state, including a
|
||||
full redraw every 2 s and whenever the display reboots.
|
||||
|
||||
## Program.s / page setup
|
||||
|
||||
In **Program.s**, before `page 0`, add:
|
||||
|
||||
```
|
||||
bauds=115200
|
||||
```
|
||||
|
||||
This sets the display to the `NEXTION_BAUD` in `remote/Config.h`. The
|
||||
display ships at 9600, and `bauds=` persists across power cycles.
|
||||
|
||||
Name the page `main` (page 0). The firmware never changes pages.
|
||||
|
||||
## Components
|
||||
|
||||
Text components need **txt_maxl** at least as large as the longest string
|
||||
the firmware writes. Button bounds here allow some slack.
|
||||
|
||||
| Name | Type | txt_maxl | Firmware writes |
|
||||
|----------|---------|----------|----------------------------------------------|
|
||||
| `bArm` | Button | 10 | `ARMED` / `DISARMED`, bco red / gray |
|
||||
| `bKill` | Button | – | nothing (static "DISARM" label, make it red) |
|
||||
| `bStand` | Button | 6 | `STAND` / `SIT`, bco green / gray |
|
||||
| `bGait` | Button | 8 | `TRIPOD` / `WAVE` |
|
||||
| `bMode` | Button | 8 | `DIRECT` / `HEADING` |
|
||||
| `bRs` | Button | 10 | `R: ROTATE` / `R: MOVE` |
|
||||
| `bRGyro` | Button | – | bco green / gray (label it "REMOTE GYRO") |
|
||||
| `bBGyro` | Button | – | bco green / gray (label it "ROBOT GYRO") |
|
||||
| `hSpeed` | Slider | – | `val` 0..255 (set **maxval = 255**) |
|
||||
| `nSpeed` | Number | – | `val` 0..100 (speed %) |
|
||||
| `bCal` | Button | 9 | `CAL` / `SAVE CAL`, bco orange while active |
|
||||
| `bCalX` | Button | – | nothing (static "CANCEL CAL") |
|
||||
| `bZero` | Button | – | nothing (static "LEVEL") |
|
||||
| `tLink` | Text | 10 | `LINK 97%` / `NO LINK`, pco green / red |
|
||||
| `tMsg` | Text | 40 | status / rejection messages |
|
||||
|
||||
## Touch events
|
||||
|
||||
Put this in each component's **Touch Release Event** (the Speed slider
|
||||
uses **Touch Move** as well so the value tracks your finger). Frame format:
|
||||
`'#'` (0x23), length, command byte, optional value byte.
|
||||
|
||||
| Component | Event code | Command |
|
||||
|-----------|------------------------------------------|--------------------------------|
|
||||
| `bArm` | `printh 23 01 41` | `A` toggle arm (needs sticks centered) |
|
||||
| `bKill` | `printh 23 01 4B` | `K` disarm, always accepted |
|
||||
| `bStand` | `printh 23 01 53` | `S` toggle sit/stand |
|
||||
| `bGait` | `printh 23 01 47` | `G` tripod ↔ wave |
|
||||
| `bMode` | `printh 23 01 4D` | `M` direct ↔ heading-hold |
|
||||
| `bRs` | `printh 23 01 58` | `X` right stick rotate ↔ translate |
|
||||
| `bRGyro` | `printh 23 01 52` | `R` remote-gyro tilt on/off |
|
||||
| `bBGyro` | `printh 23 01 42` | `B` robot self-stabilize on/off |
|
||||
| `hSpeed` | `printh 23 02 56`<br>`prints hSpeed.val,1` | `V` + speed byte |
|
||||
| `bZero` | `printh 23 01 5A` | `Z` zero handset level |
|
||||
| `bCal` | `printh 23 01 43` | `C` start / save stick calibration |
|
||||
| `bCalX` | `printh 23 01 51` | `Q` cancel calibration |
|
||||
|
||||
**Uncheck "Send Component ID"** on every component. The firmware ignores
|
||||
standard touch frames, and they only add serial traffic.
|
||||
|
||||
Protocol details and the parser live in `NextionUI.h/.cpp`. To add a
|
||||
control, pick an unused command byte, add it to `NextionUI::decodeCustom()`
|
||||
and `UiCmd`, handle it in `RemoteState::handle()`, and add a row here.
|
||||
@@ -0,0 +1,157 @@
|
||||
#include "NextionUI.h"
|
||||
#include <stdarg.h>
|
||||
|
||||
// RGB565 colors as Nextion expects them.
|
||||
static constexpr uint16_t COL_ON = 2016; // green
|
||||
static constexpr uint16_t COL_OFF = 50712; // Nextion default light gray
|
||||
static constexpr uint16_t COL_DANGER = 63488; // red
|
||||
static constexpr uint16_t COL_WARN = 64512; // orange
|
||||
|
||||
// Serial2's default 64-byte TX buffer would make a full redraw (~300 bytes
|
||||
// at 115200 baud) block the TX loop for ~25 ms. Give it room to queue.
|
||||
static uint8_t tx_extra[1024];
|
||||
static uint8_t rx_extra[128];
|
||||
|
||||
void NextionUI::begin() {
|
||||
NEXTION_SERIAL.begin(NEXTION_BAUD);
|
||||
NEXTION_SERIAL.addMemoryForWrite(tx_extra, sizeof(tx_extra));
|
||||
NEXTION_SERIAL.addMemoryForRead(rx_extra, sizeof(rx_extra));
|
||||
cmd("bkcmd=0"); // no success/fail replies - we never need them
|
||||
force_full_ = true;
|
||||
}
|
||||
|
||||
void NextionUI::cmd(const char* fmt, ...) {
|
||||
char buf[64];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
va_end(ap);
|
||||
NEXTION_SERIAL.print(buf);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
}
|
||||
|
||||
// ---- Receive ----
|
||||
|
||||
bool NextionUI::decodeCustom(UiEvent& out) {
|
||||
out.value = body_len_ > 1 ? body_[1] : 0;
|
||||
switch (body_[0]) {
|
||||
case 'A': out.cmd = UiCmd::ToggleArm; break;
|
||||
case 'K': out.cmd = UiCmd::Disarm; break;
|
||||
case 'S': out.cmd = UiCmd::ToggleStand; break;
|
||||
case 'G': out.cmd = UiCmd::ToggleGait; break;
|
||||
case 'M': out.cmd = UiCmd::ToggleWalkMode; break;
|
||||
case 'X': out.cmd = UiCmd::ToggleRStickXlate; break;
|
||||
case 'R': out.cmd = UiCmd::ToggleRemoteGyro; break;
|
||||
case 'B': out.cmd = UiCmd::ToggleRobotGyro; break;
|
||||
case 'V': out.cmd = UiCmd::SetSpeed; break;
|
||||
case 'Z': out.cmd = UiCmd::ZeroGyro; break;
|
||||
case 'C': out.cmd = UiCmd::CalStartSave; break;
|
||||
case 'Q': out.cmd = UiCmd::CalCancel; break;
|
||||
default: return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool NextionUI::poll(UiEvent& out) {
|
||||
while (NEXTION_SERIAL.available()) {
|
||||
uint8_t b = NEXTION_SERIAL.read();
|
||||
switch (rx_) {
|
||||
case Rx::Idle:
|
||||
if (b == '#') {
|
||||
rx_ = Rx::CustomLen;
|
||||
} else {
|
||||
rx_ = Rx::Native;
|
||||
native_first_ = b;
|
||||
native_len_ = 1;
|
||||
ff_run_ = (b == 0xFF) ? 1 : 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case Rx::CustomLen:
|
||||
if (b >= 1 && b <= sizeof(body_)) {
|
||||
body_len_ = b;
|
||||
body_got_ = 0;
|
||||
rx_ = Rx::CustomBody;
|
||||
} else {
|
||||
rx_ = Rx::Idle; // garbage - resync on the next '#'
|
||||
}
|
||||
break;
|
||||
|
||||
case Rx::CustomBody:
|
||||
body_[body_got_++] = b;
|
||||
if (body_got_ == body_len_) {
|
||||
rx_ = Rx::Idle;
|
||||
if (decodeCustom(out)) return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case Rx::Native:
|
||||
native_len_++;
|
||||
ff_run_ = (b == 0xFF) ? ff_run_ + 1 : 0;
|
||||
if (ff_run_ == 3) {
|
||||
// 0x88 = "Nextion ready" after power-on/reset, 0x00 = startup.
|
||||
if (native_first_ == 0x88 || native_first_ == 0x00) force_full_ = true;
|
||||
rx_ = Rx::Idle;
|
||||
} else if (native_len_ > 16) {
|
||||
rx_ = Rx::Idle; // not a frame we understand
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// ---- Render ----
|
||||
|
||||
void NextionUI::pushState(const UiView& v, bool full) {
|
||||
const UiView& s = shown_;
|
||||
if (full || v.armed != s.armed) {
|
||||
cmd("bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED");
|
||||
cmd("bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF);
|
||||
}
|
||||
if (full || v.stand != s.stand) {
|
||||
cmd("bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
|
||||
cmd("bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
|
||||
}
|
||||
if (full || v.gait_wave != s.gait_wave) cmd("bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
|
||||
if (full || v.mode_b != s.mode_b) cmd("bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
|
||||
if (full || v.rstick_xlate != s.rstick_xlate) cmd("bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
|
||||
if (full || v.remote_gyro != s.remote_gyro) cmd("bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
|
||||
if (full || v.robot_gyro != s.robot_gyro) cmd("bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
|
||||
if (full || v.speed_target != s.speed_target) {
|
||||
cmd("hSpeed.val=%u", v.speed_target);
|
||||
cmd("nSpeed.val=%u", (v.speed_target * 100 + 127) / 255);
|
||||
}
|
||||
if (full || v.calibrating != s.calibrating) {
|
||||
cmd("bCal.txt=\"%s\"", v.calibrating ? "SAVE CAL" : "CAL");
|
||||
cmd("bCal.bco=%u", v.calibrating ? COL_WARN : COL_OFF);
|
||||
}
|
||||
if (full || v.message != s.message) cmd("tMsg.txt=\"%s\"", v.message);
|
||||
}
|
||||
|
||||
void NextionUI::pushLink(const UiView& v) {
|
||||
if (v.link_ok) cmd("tLink.txt=\"LINK %u%%\"", v.link_pct);
|
||||
else cmd("tLink.txt=\"NO LINK\"");
|
||||
cmd("tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER);
|
||||
}
|
||||
|
||||
void NextionUI::render(const UiView& v, uint32_t now_ms) {
|
||||
bool full = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS;
|
||||
if (full) {
|
||||
force_full_ = false;
|
||||
last_full_ms_ = now_ms;
|
||||
}
|
||||
|
||||
pushState(v, full);
|
||||
|
||||
// Link quality changes nearly every packet; repaint it at a readable rate,
|
||||
// but immediately when the link comes or goes.
|
||||
if (full || v.link_ok != shown_.link_ok || (now_ms - last_link_ms_) >= NEXTION_LINK_REFRESH_MS) {
|
||||
pushLink(v);
|
||||
last_link_ms_ = now_ms;
|
||||
}
|
||||
|
||||
shown_ = v;
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
#include "UiTypes.h"
|
||||
#include "Config.h"
|
||||
|
||||
// Nextion serial HMI driver. No Nextion library - the protocol is small:
|
||||
//
|
||||
// Nextion -> remote: custom frames the HMI's touch events send with printh:
|
||||
// '#' <len> <cmd> [value] e.g. printh 23 01 41 (ToggleArm)
|
||||
// printh 23 02 56 + prints hSpeed.val,1
|
||||
// Command bytes are listed in NEXTION.md and in NextionUI.cpp.
|
||||
// Native Nextion return frames (terminated FF FF FF) are also parsed so a
|
||||
// display reset (0x88 "ready") triggers a full redraw.
|
||||
//
|
||||
// remote -> Nextion: plain instructions ("bArm.txt=\"ARMED\"" + FF FF FF).
|
||||
//
|
||||
// The remote is authoritative: buttons are plain buttons (not dual-state),
|
||||
// they only send requests, and render() repaints whatever the remote state
|
||||
// actually is. A Nextion reboot or a dropped byte self-heals on the next
|
||||
// periodic full refresh.
|
||||
class NextionUI {
|
||||
public:
|
||||
void begin();
|
||||
bool poll(UiEvent& out); // call every loop; returns one decoded event at a time
|
||||
void render(const UiView& v, uint32_t now_ms);
|
||||
|
||||
private:
|
||||
enum class Rx : uint8_t { Idle, CustomLen, CustomBody, Native };
|
||||
Rx rx_ = Rx::Idle;
|
||||
uint8_t body_[4];
|
||||
uint8_t body_len_ = 0, body_got_ = 0;
|
||||
uint8_t native_first_ = 0, native_len_ = 0, ff_run_ = 0;
|
||||
|
||||
UiView shown_;
|
||||
bool force_full_ = true;
|
||||
uint32_t last_full_ms_ = 0;
|
||||
uint32_t last_link_ms_ = 0;
|
||||
|
||||
void cmd(const char* fmt, ...);
|
||||
void pushState(const UiView& v, bool full);
|
||||
void pushLink(const UiView& v);
|
||||
bool decodeCustom(UiEvent& out);
|
||||
};
|
||||
@@ -0,0 +1,69 @@
|
||||
#include "PacketBuilder.h"
|
||||
|
||||
// Handset tilt (deg) -> -1000..1000 with a small deadzone for hand tremor,
|
||||
// scaled by the fade blend so toggling remote gyro ramps instead of steps.
|
||||
int16_t PacketBuilder::tiltToWire(float deg, float blend) {
|
||||
float mag = fabsf(deg) - REMOTE_TILT_DEADZONE_DEG;
|
||||
if (mag <= 0 || blend <= 0) return 0;
|
||||
float n = constrain(mag / (REMOTE_TILT_FULL_SCALE_DEG - REMOTE_TILT_DEADZONE_DEG), 0.0f, 1.0f);
|
||||
return (int16_t)lroundf(copysignf(n, deg) * 1000.0f * blend);
|
||||
}
|
||||
|
||||
CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms) {
|
||||
CmdPacket p;
|
||||
p.seq = seq_++;
|
||||
p.speed_scale = st.speedCurrent();
|
||||
|
||||
uint8_t f = 0;
|
||||
if (st.armed()) f |= FLAG_ARMED;
|
||||
if (st.standRequested()) f |= FLAG_STAND;
|
||||
if (st.modeB()) f |= FLAG_MODE_B;
|
||||
if (st.rstickXlate()) f |= FLAG_RSTICK_XLATE;
|
||||
if (st.gaitWave()) f |= FLAG_GAIT_WAVE;
|
||||
if (st.robotGyro()) f |= FLAG_ROBOT_GYRO;
|
||||
|
||||
// Disarmed: send a neutral command. The robot ignores motion then anyway,
|
||||
// but this keeps its pose smoothers from winding up to a stale stick.
|
||||
if (!st.armed()) {
|
||||
p.flags = f;
|
||||
return p;
|
||||
}
|
||||
|
||||
// ---- Left stick: walking. Handset stick frame -> body frame:
|
||||
// stick forward (+Y) -> +vx forward, stick right (+X) -> -vy (vy + = left),
|
||||
// twist CCW -> +yaw_rate (turn left).
|
||||
p.vx = s.v[L_Y];
|
||||
p.vy = -s.v[L_X];
|
||||
p.yaw_rate = s.v[L_TWIST];
|
||||
|
||||
// ---- Right stick: body pose. Each packet field is "about / along" one body
|
||||
// axis, so the physical-axis mapping differs per sub-mode:
|
||||
if (st.rstickXlate()) {
|
||||
p.rx = s.v[R_Y]; // X offset: stick forward -> body forward
|
||||
p.ry = -s.v[R_X]; // Y offset: stick right -> body right (-Y)
|
||||
p.rz = s.v[R_TWIST]; // Z offset: twist CCW -> body up
|
||||
} else {
|
||||
p.rx = s.v[R_X]; // roll: stick right -> right side down
|
||||
p.ry = s.v[R_Y]; // pitch: stick forward -> nose down
|
||||
p.rz = s.v[R_TWIST]; // yaw: twist CCW -> body yaws left
|
||||
}
|
||||
|
||||
// ---- Walk flag: in mode B the robot ignores yaw_rate, so twist alone
|
||||
// shouldn't start stepping. Linger avoids STAND<->WALKING chatter.
|
||||
bool walk_input = p.vx != 0 || p.vy != 0 || (!st.modeB() && p.yaw_rate != 0);
|
||||
if (walk_input) { walk_seen_ = true; last_walk_input_ms_ = now_ms; }
|
||||
bool walking = walk_seen_ && (now_ms - last_walk_input_ms_) < WALK_LINGER_MS;
|
||||
if (st.standRequested() && (walk_input || walking)) f |= FLAG_WALK;
|
||||
|
||||
// ---- Remote tilt feedforward: keep the flag up for the whole fade-out so
|
||||
// the robot sees the tilt ramp to zero rather than being cut off.
|
||||
float blend = st.remoteTiltBlend();
|
||||
if (st.remoteGyro() || blend > 0.0f) {
|
||||
f |= FLAG_REMOTE_GYRO;
|
||||
p.remote_roll = tiltToWire(imu.rollDeg(), blend);
|
||||
p.remote_pitch = tiltToWire(imu.pitchDeg(), blend);
|
||||
}
|
||||
|
||||
p.flags = f;
|
||||
return p;
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#include <HexapodProtocol.h>
|
||||
#include "Sticks.h"
|
||||
#include "RemoteIMU.h"
|
||||
#include "RemoteState.h"
|
||||
|
||||
// The inverse of the robot's InputMapper: turns physical stick axes, handset
|
||||
// tilt and operator state into a CmdPacket. It only fills fields per the
|
||||
// protocol contract (HexapodProtocol.h); all physical scaling and mode
|
||||
// interpretation stays on the robot.
|
||||
class PacketBuilder {
|
||||
public:
|
||||
CmdPacket build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms);
|
||||
|
||||
private:
|
||||
uint8_t seq_ = 0;
|
||||
bool walk_seen_ = false;
|
||||
uint32_t last_walk_input_ms_ = 0;
|
||||
|
||||
static int16_t tiltToWire(float deg, float blend);
|
||||
};
|
||||
@@ -0,0 +1,112 @@
|
||||
# Hexapod remote firmware (Teensy 4.1)
|
||||
|
||||
Handheld transmitter for the hexapod. It reads two 3-axis twist-grip
|
||||
joysticks and an MPU6050, takes operator commands from a Nextion
|
||||
touchscreen, and sends a `CmdPacket` (defined in
|
||||
`../shared/HexapodProtocol/HexapodProtocol.h`) to the robot over nRF24L01
|
||||
at 100 Hz.
|
||||
|
||||
## Building
|
||||
|
||||
From the repo root: `pio run -e remote`, flash with
|
||||
`pio run -e remote -t upload`, monitor with `pio device monitor -e remote`.
|
||||
|
||||
## Wiring (placeholders; edit `Config.h` to match)
|
||||
|
||||
| Part | Signal | Teensy 4.1 pin |
|
||||
|------|--------|----------------|
|
||||
| nRF24L01 | MOSI / MISO / SCK | 11 / 12 / 13 (SPI0) |
|
||||
| nRF24L01 | CE / CSN | 9 / 10 |
|
||||
| nRF24L01 | VCC | 3.3 V, with a 10–100 µF cap right at the module |
|
||||
| MPU6050 | SDA / SCL | 18 / 19 (Wire) |
|
||||
| Nextion | TX → RX2, RX ← TX2 | 7 / 8 (Serial2), 5 V supply |
|
||||
| Left stick | X / Y / twist | A0 / A1 / A2 |
|
||||
| Right stick | X / Y / twist | A3 / A6 / A7 |
|
||||
|
||||
Stick pots are wired 3.3 V – wiper – GND. **Never feed 5 V into a Teensy
|
||||
4.x analog pin.**
|
||||
|
||||
## Control scheme
|
||||
|
||||
| Input | Packet field | Meaning on the robot |
|
||||
|-------|--------------|----------------------|
|
||||
| Left stick forward/back | `vx` | walk forward/back |
|
||||
| Left stick left/right | `vy` | strafe (mode A); heading target direction (mode B) |
|
||||
| Left stick twist | `yaw_rate` | turn rate (mode A only; ignored in mode B) |
|
||||
| Right stick, **ROTATE** (default) | `rx/ry/rz` | body roll / pitch / yaw |
|
||||
| Right stick, **MOVE** (`FLAG_RSTICK_XLATE`) | `rx/ry/rz` | body X / Y / Z (height, via twist) offset |
|
||||
| Handset tilt | `remote_roll/pitch` | extra body roll/pitch while REMOTE GYRO is on |
|
||||
|
||||
Touchscreen: ARM / DISARM, SIT/STAND, TRIPOD/WAVE, DIRECT/HEADING,
|
||||
R: ROTATE/MOVE, REMOTE GYRO, ROBOT GYRO, speed slider, LEVEL (re-zero the
|
||||
handset tilt), CAL / CANCEL CAL. `NEXTION.md` has the HMI build guide.
|
||||
|
||||
### Safety interlocks (in `RemoteState`)
|
||||
|
||||
- **Arming** is refused unless every stick axis is centered, and never
|
||||
allowed during calibration.
|
||||
- **Disarming** always clears the stand request. The robot arms into SIT,
|
||||
so re-arming never stands it up by itself.
|
||||
- **Link loss:** if no ack arrives for `LINK_LOST_MS` (300 ms, matching the
|
||||
robot's failsafe), the remote disarms itself. When the link comes back
|
||||
the robot stays down until you re-arm deliberately.
|
||||
- **Sit/stand** requests are only accepted while armed. `FLAG_WALK` is only
|
||||
sent while standing is requested.
|
||||
- While disarmed or calibrating, all motion fields are sent as zero.
|
||||
|
||||
### Smoothing
|
||||
|
||||
- **Speed scale** ramps toward the slider at `SPEED_SCALE_RATE_PER_SEC`.
|
||||
The robot uses speed scale directly on the walking twist, so a slider
|
||||
jump would otherwise be a velocity step.
|
||||
- **Remote tilt** fades in/out over ~0.5 s when REMOTE GYRO is toggled, and
|
||||
the handset re-zeroes its "level" at the moment you enable it.
|
||||
- **FLAG_WALK** lingers 250 ms after the left stick centers, so hovering at
|
||||
the deadzone edge doesn't make the robot chatter between STAND and
|
||||
WALKING.
|
||||
- Body pose smoothing is left to the robot's `BodyPose` rate limiters.
|
||||
|
||||
## Stick calibration
|
||||
|
||||
1. Power on with both sticks released and the remote held still. Each
|
||||
axis's resting center is captured automatically.
|
||||
2. For full calibration, disarm, then tap **CAL** (or press `c` on the
|
||||
serial console). Move every axis of both sticks to both limits, release
|
||||
them, and tap **SAVE CAL** (`c`). The result is saved to EEPROM.
|
||||
3. If any axis moved less than `CAL_MIN_HALF_SPAN` either side of center,
|
||||
the save is refused and you stay in calibration mode. **CANCEL CAL**
|
||||
(`x`) aborts.
|
||||
4. `p` prints the calibration in a form you can paste into `Config.h`
|
||||
defaults. `R` wipes EEPROM back to the `Config.h` defaults.
|
||||
|
||||
## Serial console (USB, 115200, single keystrokes)
|
||||
|
||||
Every touchscreen control is available here, so you can bench-test with no
|
||||
display attached.
|
||||
|
||||
| Key | Action | Key | Action |
|
||||
|-----|--------|-----|--------|
|
||||
| `a` | toggle arm | `k` | disarm |
|
||||
| `s` | toggle sit/stand | `g` | tripod ↔ wave |
|
||||
| `m` | direct ↔ heading-hold | `t` | right stick rotate ↔ translate |
|
||||
| `r` | remote gyro on/off | `b` | robot gyro on/off |
|
||||
| `+` / `-` | speed ±25 | `z` | zero handset level |
|
||||
| `c` | start / save calibration | `x` | cancel calibration |
|
||||
| `p` | print status (raw + mapped sticks, IMU, link, state, cal) | `d` | toggle 10 Hz packet dump |
|
||||
| `R` | reset stick cal to Config defaults | | |
|
||||
|
||||
## Bench-check checklist
|
||||
|
||||
1. **Stick directions:** with `p`, confirm each logical axis reads
|
||||
positive for right / forward / counterclockwise twist. Flip it in
|
||||
`STICK_INVERT[]` if not.
|
||||
2. **IMU mounting:** with `p`, dip the right edge down to get +roll, and
|
||||
tip the far edge down to get +pitch. If not, fix `IMU_AXIS_FROM_CHIP` /
|
||||
`IMU_AXIS_SIGN`.
|
||||
3. **Nextion baud:** `bauds=115200` must be in Program.s, or the display
|
||||
shows nothing useful.
|
||||
4. **Radio:** with the robot powered, `tLink` / `p` should show ~100%. If
|
||||
it reads NO LINK, check that both boards were built from the same
|
||||
`HexapodProtocol.h`. The static payload size is part of the handshake.
|
||||
5. **End to end with the robot on a stand:** use `d` to dump packets and
|
||||
confirm each field changes the way the table above says.
|
||||
@@ -0,0 +1,42 @@
|
||||
#include "RadioLink.h"
|
||||
#include "Config.h"
|
||||
#include <RF24.h>
|
||||
#include <SPI.h>
|
||||
|
||||
static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_PA_LOW,
|
||||
"HexapodProtocol.h radio enum values out of sync with RF24.h");
|
||||
|
||||
static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
|
||||
|
||||
bool RadioLink::begin() {
|
||||
present_ = radio.begin();
|
||||
if (present_) {
|
||||
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // must match the robot exactly (static payloads)
|
||||
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
|
||||
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
|
||||
radio.setChannel(HXP_RADIO_CHANNEL);
|
||||
radio.setRetries(RADIO_RETRY_DELAY, RADIO_RETRY_COUNT);
|
||||
radio.openWritingPipe(HXP_RADIO_ADDRESS);
|
||||
radio.stopListening();
|
||||
}
|
||||
return present_;
|
||||
}
|
||||
|
||||
bool RadioLink::send(const CmdPacket& p) {
|
||||
if (!present_) return false;
|
||||
bool acked = radio.write(&p, sizeof(p));
|
||||
history_ = (history_ << 1) | (acked ? 1u : 0u);
|
||||
if (acked) {
|
||||
last_ack_ms_ = millis();
|
||||
ever_acked_ = true;
|
||||
}
|
||||
return acked;
|
||||
}
|
||||
|
||||
void RadioLink::update(uint32_t now_ms) {
|
||||
link_ok_ = present_ && ever_acked_ && (now_ms - last_ack_ms_) < LINK_LOST_MS;
|
||||
}
|
||||
|
||||
uint8_t RadioLink::qualityPct() const {
|
||||
return (uint8_t)(__builtin_popcount(history_) * 100 / 32);
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
#include <HexapodProtocol.h>
|
||||
|
||||
// nRF24L01 transmitter. Uses hardware auto-ack to measure the link: the
|
||||
// robot's radio acks every packet it receives, so an ack = the robot heard us.
|
||||
class RadioLink {
|
||||
public:
|
||||
bool begin(); // false if the radio chip didn't respond
|
||||
bool send(const CmdPacket& p); // blocking, returns true if acked
|
||||
void update(uint32_t now_ms); // refreshes linkOk()
|
||||
|
||||
bool present() const { return present_; }
|
||||
bool linkOk() const { return link_ok_; }
|
||||
uint8_t qualityPct() const; // acked share of the last 32 packets
|
||||
|
||||
private:
|
||||
bool present_ = false;
|
||||
bool link_ok_ = false;
|
||||
uint32_t last_ack_ms_ = 0;
|
||||
bool ever_acked_ = false;
|
||||
uint32_t history_ = 0; // 1 bit per packet, newest in bit 0
|
||||
};
|
||||
@@ -0,0 +1,81 @@
|
||||
#include "RemoteIMU.h"
|
||||
#include "Config.h"
|
||||
#include <Wire.h>
|
||||
|
||||
// MPU6050 register map, default +/-2g / +/-250deg/s ranges (same part and
|
||||
// scale factors as robot/RobotIMU.cpp).
|
||||
static constexpr uint8_t REG_CONFIG = 0x1A;
|
||||
static constexpr uint8_t REG_PWR_MGMT_1 = 0x6B;
|
||||
static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B;
|
||||
|
||||
static bool writeReg(uint8_t reg, uint8_t val) {
|
||||
Wire.beginTransmission(IMU_ADDR);
|
||||
Wire.write(reg);
|
||||
Wire.write(val);
|
||||
return Wire.endTransmission() == 0;
|
||||
}
|
||||
|
||||
bool RemoteIMU::begin() {
|
||||
Wire.begin();
|
||||
Wire.setClock(400000);
|
||||
ok_ = writeReg(REG_PWR_MGMT_1, 0x00); // wake out of sleep
|
||||
if (!ok_) return false;
|
||||
writeReg(REG_CONFIG, 0x03); // 44 Hz DLPF: a handheld remote doesn't need more bandwidth
|
||||
delay(50);
|
||||
|
||||
// Gyro bias: average while the remote sits still at power-on.
|
||||
float sum[3] = {};
|
||||
int got = 0;
|
||||
for (int n = 0; n < IMU_GYRO_BIAS_SAMPLES; n++) {
|
||||
float a[3], g[3];
|
||||
if (readRaw(a, g)) {
|
||||
for (int k = 0; k < 3; k++) sum[k] += g[k];
|
||||
got++;
|
||||
}
|
||||
delay(2);
|
||||
}
|
||||
if (got > 0)
|
||||
for (int k = 0; k < 3; k++) gyro_bias_[k] = sum[k] / got;
|
||||
|
||||
// Seed the filter from the accelerometer so it doesn't slew in from 0,
|
||||
// and treat the power-on attitude as level until the operator re-zeroes.
|
||||
float a[3], g[3];
|
||||
if (readRaw(a, g)) {
|
||||
roll_deg_ = degrees(atan2f(a[1], a[2]));
|
||||
pitch_deg_ = degrees(atan2f(-a[0], sqrtf(a[1] * a[1] + a[2] * a[2])));
|
||||
}
|
||||
zeroLevel();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool RemoteIMU::readRaw(float a[3], float g[3]) {
|
||||
Wire.beginTransmission(IMU_ADDR);
|
||||
Wire.write(REG_ACCEL_XOUT_H);
|
||||
if (Wire.endTransmission(false) != 0) return false;
|
||||
Wire.requestFrom((int)IMU_ADDR, 14);
|
||||
if (Wire.available() < 14) return false;
|
||||
|
||||
int16_t raw[7];
|
||||
for (int k = 0; k < 7; k++) raw[k] = (Wire.read() << 8) | Wire.read(); // ax ay az temp gx gy gz
|
||||
|
||||
float chip_a[3] = { raw[0] / 16384.0f, raw[1] / 16384.0f, raw[2] / 16384.0f };
|
||||
float chip_g[3] = { raw[4] / 131.0f, raw[5] / 131.0f, raw[6] / 131.0f };
|
||||
|
||||
for (int k = 0; k < 3; k++) {
|
||||
a[k] = chip_a[IMU_AXIS_FROM_CHIP[k]] * IMU_AXIS_SIGN[k];
|
||||
g[k] = chip_g[IMU_AXIS_FROM_CHIP[k]] * IMU_AXIS_SIGN[k] - gyro_bias_[k];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void RemoteIMU::update(float dt) {
|
||||
if (!ok_) return;
|
||||
float a[3], g[3];
|
||||
if (!readRaw(a, g)) return;
|
||||
|
||||
float accel_roll = degrees(atan2f(a[1], a[2]));
|
||||
float accel_pitch = degrees(atan2f(-a[0], sqrtf(a[1] * a[1] + a[2] * a[2])));
|
||||
|
||||
roll_deg_ = IMU_ALPHA * (roll_deg_ + g[0] * dt) + (1 - IMU_ALPHA) * accel_roll;
|
||||
pitch_deg_ = IMU_ALPHA * (pitch_deg_ + g[1] * dt) + (1 - IMU_ALPHA) * accel_pitch;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Handset attitude from an MPU6050: same complementary filter as the robot's
|
||||
// RobotIMU, plus a mounting-orientation remap, boot-time gyro bias capture,
|
||||
// and a "level" reference so the operator's natural holding angle reads as 0.
|
||||
class RemoteIMU {
|
||||
public:
|
||||
bool begin(); // false if the IMU didn't respond; hold the remote still during this
|
||||
void update(float dt); // no-op if the IMU never came up
|
||||
bool ok() const { return ok_; }
|
||||
|
||||
// Tilt relative to the zeroed level pose, degrees, handset frame
|
||||
// (+roll = right side down, +pitch = far edge down).
|
||||
float rollDeg() const { return roll_deg_ - zero_roll_; }
|
||||
float pitchDeg() const { return pitch_deg_ - zero_pitch_; }
|
||||
|
||||
void zeroLevel() { zero_roll_ = roll_deg_; zero_pitch_ = pitch_deg_; }
|
||||
|
||||
private:
|
||||
float roll_deg_ = 0, pitch_deg_ = 0;
|
||||
float zero_roll_ = 0, zero_pitch_ = 0;
|
||||
float gyro_bias_[3] = {};
|
||||
bool ok_ = false;
|
||||
|
||||
// Reads and remaps into the handset frame. Accel in g, gyro in deg/s.
|
||||
bool readRaw(float a[3], float g[3]);
|
||||
};
|
||||
@@ -0,0 +1,114 @@
|
||||
#include "RemoteState.h"
|
||||
|
||||
void RemoteState::begin() {
|
||||
speed_.reset(speed_target_);
|
||||
tilt_blend_.reset(0);
|
||||
}
|
||||
|
||||
void RemoteState::setMessage(const char* msg, uint32_t now_ms) {
|
||||
message_ = msg;
|
||||
message_until_ms_ = now_ms + STATUS_MESSAGE_MS;
|
||||
}
|
||||
|
||||
// Disarming always clears the stand request: the robot arms into SIT, and
|
||||
// re-arming must never make it stand up on its own.
|
||||
void RemoteState::disarm() {
|
||||
armed_ = false;
|
||||
stand_ = false;
|
||||
}
|
||||
|
||||
bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) {
|
||||
const StickFrame& sticks = stick_hw.frame();
|
||||
switch (e.cmd) {
|
||||
case UiCmd::None:
|
||||
return false;
|
||||
|
||||
case UiCmd::Disarm:
|
||||
disarm();
|
||||
return true;
|
||||
|
||||
case UiCmd::ToggleArm:
|
||||
if (armed_) { disarm(); return true; }
|
||||
if (calibrating_) { setMessage("Finish calibration first", now); return false; }
|
||||
if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; }
|
||||
armed_ = true;
|
||||
return true;
|
||||
|
||||
case UiCmd::ToggleStand:
|
||||
if (!armed_) { setMessage("Arm first", now); return false; }
|
||||
stand_ = !stand_;
|
||||
return true;
|
||||
|
||||
case UiCmd::ToggleGait: gait_wave_ = !gait_wave_; return true;
|
||||
case UiCmd::ToggleWalkMode: mode_b_ = !mode_b_; return true;
|
||||
case UiCmd::ToggleRStickXlate: rstick_xlate_ = !rstick_xlate_; return true;
|
||||
case UiCmd::ToggleRobotGyro: robot_gyro_ = !robot_gyro_; return true;
|
||||
|
||||
case UiCmd::ToggleRemoteGyro:
|
||||
if (!remote_gyro_ && !imu.ok()) { setMessage("Remote IMU not detected", now); return false; }
|
||||
remote_gyro_ = !remote_gyro_;
|
||||
// Re-zero on enable: whatever angle you're holding the remote at when
|
||||
// you switch it on becomes "level", so the body doesn't lurch.
|
||||
if (remote_gyro_) imu.zeroLevel();
|
||||
return true;
|
||||
|
||||
case UiCmd::SetSpeed: speed_target_ = e.value; return true;
|
||||
case UiCmd::SpeedUp: speed_target_ = (uint8_t)min(255, speed_target_ + SPEED_SCALE_STEP); return true;
|
||||
case UiCmd::SpeedDown: speed_target_ = (uint8_t)max(0, speed_target_ - SPEED_SCALE_STEP); return true;
|
||||
|
||||
case UiCmd::ZeroGyro:
|
||||
imu.zeroLevel();
|
||||
setMessage("Remote level zeroed", now);
|
||||
return true;
|
||||
|
||||
case UiCmd::CalStartSave:
|
||||
if (!calibrating_) {
|
||||
if (armed_) { setMessage("Disarm to calibrate", now); return false; }
|
||||
stick_hw.startCalibration();
|
||||
calibrating_ = true;
|
||||
setMessage("Sweep all axes to limits, release, SAVE", now);
|
||||
return true;
|
||||
}
|
||||
if (!stick_hw.saveCalibration()) { setMessage("Sweep too small - keep sweeping", now); return false; }
|
||||
calibrating_ = false;
|
||||
setMessage("Calibration saved", now);
|
||||
return true;
|
||||
|
||||
case UiCmd::CalCancel:
|
||||
if (!calibrating_) return false;
|
||||
stick_hw.cancelCalibration();
|
||||
calibrating_ = false;
|
||||
setMessage("Calibration cancelled", now);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void RemoteState::update(float dt, bool link_ok, uint32_t now_ms) {
|
||||
// Mirror the robot's link failsafe: it disarms (servos relax) when packets
|
||||
// stop, so the remote disarms too and the operator must re-arm on purpose
|
||||
// once the link is back.
|
||||
if (!link_ok && link_was_ok_ && armed_) {
|
||||
disarm();
|
||||
setMessage("Link lost - disarmed", now_ms);
|
||||
}
|
||||
link_was_ok_ = link_ok;
|
||||
|
||||
speed_.setTarget(speed_target_);
|
||||
speed_.update(dt);
|
||||
tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f);
|
||||
tilt_blend_.update(dt);
|
||||
}
|
||||
|
||||
void RemoteState::fillView(UiView& v, uint32_t now_ms) const {
|
||||
v.armed = armed_;
|
||||
v.stand = stand_;
|
||||
v.gait_wave = gait_wave_;
|
||||
v.mode_b = mode_b_;
|
||||
v.rstick_xlate = rstick_xlate_;
|
||||
v.remote_gyro = remote_gyro_;
|
||||
v.robot_gyro = robot_gyro_;
|
||||
v.calibrating = calibrating_;
|
||||
v.speed_target = speed_target_;
|
||||
v.message = message(now_ms);
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
#include "UiTypes.h"
|
||||
#include "Sticks.h"
|
||||
#include "RemoteIMU.h"
|
||||
#include "SmoothedValue.h"
|
||||
#include "Config.h"
|
||||
|
||||
// Operator-facing state of the remote: every toggle the touchscreen can
|
||||
// flip, the interlocks around them, and the smoothed setpoints. It does not
|
||||
// know the wire format - PacketBuilder turns this into a CmdPacket.
|
||||
class RemoteState {
|
||||
public:
|
||||
void begin();
|
||||
|
||||
// Applies one operator request. Sticks are needed for the arm interlock
|
||||
// and calibration, the IMU for re-zeroing. Returns true if accepted;
|
||||
// rejections leave an explanation in message().
|
||||
bool handle(const UiEvent& e, Sticks& sticks, RemoteIMU& imu, uint32_t now_ms);
|
||||
|
||||
// Advances smoothers and link-loss handling. Call once per TX tick.
|
||||
void update(float dt, bool link_ok, uint32_t now_ms);
|
||||
|
||||
bool armed() const { return armed_; }
|
||||
bool standRequested() const { return stand_; }
|
||||
bool gaitWave() const { return gait_wave_; }
|
||||
bool modeB() const { return mode_b_; }
|
||||
bool rstickXlate() const { return rstick_xlate_; }
|
||||
bool remoteGyro() const { return remote_gyro_; }
|
||||
bool robotGyro() const { return robot_gyro_; }
|
||||
|
||||
uint8_t speedTarget() const { return speed_target_; }
|
||||
uint8_t speedCurrent() const { return (uint8_t)lroundf(speed_.current); }
|
||||
float remoteTiltBlend() const { return tilt_blend_.current; } // 0..1, fades on the gyro toggle
|
||||
|
||||
const char* message(uint32_t now_ms) const { return now_ms < message_until_ms_ ? message_ : ""; }
|
||||
void setMessage(const char* msg, uint32_t now_ms);
|
||||
|
||||
void fillView(UiView& v, uint32_t now_ms) const;
|
||||
|
||||
private:
|
||||
bool armed_ = false;
|
||||
bool stand_ = false;
|
||||
bool gait_wave_ = false;
|
||||
bool mode_b_ = false;
|
||||
bool rstick_xlate_ = false;
|
||||
bool remote_gyro_ = false;
|
||||
bool robot_gyro_ = false;
|
||||
bool calibrating_ = false;
|
||||
|
||||
uint8_t speed_target_ = SPEED_SCALE_DEFAULT;
|
||||
SmoothedValue speed_{SPEED_SCALE_RATE_PER_SEC};
|
||||
SmoothedValue tilt_blend_{REMOTE_TILT_FADE_PER_SEC};
|
||||
|
||||
bool link_was_ok_ = false;
|
||||
const char* message_ = "";
|
||||
uint32_t message_until_ms_ = 0;
|
||||
|
||||
void disarm();
|
||||
};
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "SerialConsole.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
bool SerialConsole::poll(UiEvent& ui, Local& local) {
|
||||
ui = UiEvent{};
|
||||
local = Local::None;
|
||||
while (Serial.available()) {
|
||||
char c = Serial.read();
|
||||
switch (c) {
|
||||
case 'a': ui.cmd = UiCmd::ToggleArm; return true;
|
||||
case 'k': ui.cmd = UiCmd::Disarm; return true;
|
||||
case 's': ui.cmd = UiCmd::ToggleStand; return true;
|
||||
case 'g': ui.cmd = UiCmd::ToggleGait; return true;
|
||||
case 'm': ui.cmd = UiCmd::ToggleWalkMode; return true;
|
||||
case 't': ui.cmd = UiCmd::ToggleRStickXlate; return true;
|
||||
case 'r': ui.cmd = UiCmd::ToggleRemoteGyro; return true;
|
||||
case 'b': ui.cmd = UiCmd::ToggleRobotGyro; return true;
|
||||
case '+': case '=': ui.cmd = UiCmd::SpeedUp; return true;
|
||||
case '-': case '_': ui.cmd = UiCmd::SpeedDown; return true;
|
||||
case 'z': ui.cmd = UiCmd::ZeroGyro; return true;
|
||||
case 'c': ui.cmd = UiCmd::CalStartSave; return true;
|
||||
case 'x': ui.cmd = UiCmd::CalCancel; return true;
|
||||
case 'p': local = Local::PrintStatus; return true;
|
||||
case 'd': local = Local::ToggleDump; return true;
|
||||
case 'R': local = Local::ResetCal; return true;
|
||||
default: break; // ignore line endings and unknown keys
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
#include "UiTypes.h"
|
||||
|
||||
// USB serial console (115200). Mirrors every touchscreen control with a
|
||||
// single keystroke, so the remote can be bench-tested with no Nextion
|
||||
// attached, plus a few diagnostics. Key map in remote/README.md.
|
||||
class SerialConsole {
|
||||
public:
|
||||
enum class Local : uint8_t { None, PrintStatus, ToggleDump, ResetCal };
|
||||
|
||||
// Returns true with either `ui` set (a normal operator command) or
|
||||
// `local` set (a console-only diagnostic the sketch handles itself).
|
||||
bool poll(UiEvent& ui, Local& local);
|
||||
};
|
||||
@@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
// Rate-limited setpoint smoother - identical to robot/SmoothedValue.h (kept
|
||||
// as a copy rather than moved to shared/ so the robot sketch stays untouched).
|
||||
// Used for UI-driven setpoints that would otherwise step: speed scale and
|
||||
// the remote-tilt fade.
|
||||
struct SmoothedValue {
|
||||
float current = 0;
|
||||
float target = 0;
|
||||
float max_rate; // units per second
|
||||
|
||||
explicit SmoothedValue(float rate = 100.0f) : max_rate(rate) {}
|
||||
|
||||
void setTarget(float t) { target = t; }
|
||||
|
||||
float update(float dt) {
|
||||
float delta = target - current;
|
||||
float max_step = max_rate * dt;
|
||||
if (delta > max_step) delta = max_step;
|
||||
else if (delta < -max_step) delta = -max_step;
|
||||
current += delta;
|
||||
return current;
|
||||
}
|
||||
|
||||
void reset(float v) { current = target = v; }
|
||||
};
|
||||
@@ -0,0 +1,155 @@
|
||||
#include "Sticks.h"
|
||||
#include <EEPROM.h>
|
||||
|
||||
// EEPROM layout: one record at address 0. Bump CAL_VERSION if AxisCal or
|
||||
// the axis order ever changes so stale data is ignored instead of misread.
|
||||
static constexpr uint32_t CAL_MAGIC = 0x48585043; // "HXPC"
|
||||
static constexpr uint16_t CAL_VERSION = 1;
|
||||
static constexpr int CAL_EEPROM_ADDR = 0;
|
||||
|
||||
struct StoredCal {
|
||||
uint32_t magic;
|
||||
uint16_t version;
|
||||
AxisCal axes[NUM_AXES];
|
||||
uint16_t checksum;
|
||||
};
|
||||
|
||||
static uint16_t checksumOf(const StoredCal& s) {
|
||||
const uint8_t* p = reinterpret_cast<const uint8_t*>(&s);
|
||||
uint16_t sum = 0;
|
||||
for (size_t i = 0; i < offsetof(StoredCal, checksum); i++) sum = (sum << 1 | sum >> 15) ^ p[i];
|
||||
return sum;
|
||||
}
|
||||
|
||||
bool StickFrame::allCentered(int16_t tolerance) const {
|
||||
for (int i = 0; i < NUM_AXES; i++)
|
||||
if (abs(v[i]) > tolerance) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Sticks::begin() {
|
||||
analogReadResolution(ADC_BITS);
|
||||
analogReadAveraging(ADC_AVERAGING);
|
||||
|
||||
if (!loadFromEeprom()) applyDefaults();
|
||||
|
||||
bool all_ok = true;
|
||||
for (int i = 0; i < NUM_AXES; i++) {
|
||||
int16_t rest = readAveraged(i, 32);
|
||||
filtered_[i] = rest;
|
||||
if (abs(rest - cal_[i].center) <= CENTER_CAPTURE_TOLERANCE) {
|
||||
cal_[i].center = rest;
|
||||
} else {
|
||||
all_ok = false; // someone's holding a stick, or the stored cal is off - keep stored center
|
||||
}
|
||||
}
|
||||
return all_ok;
|
||||
}
|
||||
|
||||
int16_t Sticks::readAveraged(int axis, int samples) {
|
||||
int32_t sum = 0;
|
||||
for (int n = 0; n < samples; n++) sum += analogRead(STICK_PINS[axis]);
|
||||
return (int16_t)(sum / samples);
|
||||
}
|
||||
|
||||
// Raw counts -> -1..1 (piecewise around center, so asymmetric pots still
|
||||
// reach full scale both ways) -> invert -> deadzone with rescale -> expo.
|
||||
int16_t Sticks::mapAxis(int axis, float raw) const {
|
||||
const AxisCal& c = cal_[axis];
|
||||
float n;
|
||||
if (raw >= c.center) n = (raw - c.center) / (float)max(1, c.max - c.center);
|
||||
else n = (raw - c.center) / (float)max(1, c.center - c.min);
|
||||
n = constrain(n, -1.0f, 1.0f);
|
||||
if (STICK_INVERT[axis]) n = -n;
|
||||
|
||||
float dz = STICK_DEADZONE[axis];
|
||||
float mag = fabsf(n);
|
||||
if (mag <= dz) return 0;
|
||||
mag = (mag - dz) / (1.0f - dz);
|
||||
|
||||
float e = STICK_EXPO[axis];
|
||||
mag = (1.0f - e) * mag + e * mag * mag * mag;
|
||||
|
||||
return (int16_t)lroundf(copysignf(mag, n) * 1000.0f);
|
||||
}
|
||||
|
||||
void Sticks::update() {
|
||||
for (int i = 0; i < NUM_AXES; i++) {
|
||||
float r = analogRead(STICK_PINS[i]);
|
||||
filtered_[i] += STICK_FILTER_ALPHA * (r - filtered_[i]);
|
||||
|
||||
if (calibrating_) {
|
||||
int16_t v = (int16_t)filtered_[i];
|
||||
sweep_[i].min = min(sweep_[i].min, v);
|
||||
sweep_[i].max = max(sweep_[i].max, v);
|
||||
frame_.v[i] = 0; // never command motion while the operator is sweeping sticks
|
||||
} else {
|
||||
frame_.v[i] = mapAxis(i, filtered_[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Sticks::startCalibration() {
|
||||
for (int i = 0; i < NUM_AXES; i++) {
|
||||
int16_t v = (int16_t)filtered_[i];
|
||||
sweep_[i] = { v, v, v };
|
||||
}
|
||||
calibrating_ = true;
|
||||
}
|
||||
|
||||
bool Sticks::saveCalibration() {
|
||||
if (!calibrating_) return false;
|
||||
AxisCal next[NUM_AXES];
|
||||
for (int i = 0; i < NUM_AXES; i++) {
|
||||
// Sticks are released when SAVE is pressed, so the current reading is center.
|
||||
int16_t center = (int16_t)filtered_[i];
|
||||
if (center - sweep_[i].min < CAL_MIN_HALF_SPAN || sweep_[i].max - center < CAL_MIN_HALF_SPAN)
|
||||
return false; // stay in calibration mode so the operator can keep sweeping
|
||||
// Pull the ends in slightly so full deflection reliably reaches 1000.
|
||||
int16_t inset_lo = (center - sweep_[i].min) / 50;
|
||||
int16_t inset_hi = (sweep_[i].max - center) / 50;
|
||||
next[i] = { (int16_t)(sweep_[i].min + inset_lo), center, (int16_t)(sweep_[i].max - inset_hi) };
|
||||
}
|
||||
memcpy(cal_, next, sizeof(cal_));
|
||||
saveToEeprom();
|
||||
calibrating_ = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
void Sticks::cancelCalibration() { calibrating_ = false; }
|
||||
|
||||
void Sticks::applyDefaults() {
|
||||
for (int i = 0; i < NUM_AXES; i++)
|
||||
cal_[i] = { STICK_DEFAULT_MIN, STICK_DEFAULT_CENTER, STICK_DEFAULT_MAX };
|
||||
}
|
||||
|
||||
void Sticks::resetCalibrationToDefaults() {
|
||||
applyDefaults();
|
||||
StoredCal blank{};
|
||||
EEPROM.put(CAL_EEPROM_ADDR, blank); // magic = 0 -> ignored on next boot
|
||||
}
|
||||
|
||||
bool Sticks::loadFromEeprom() {
|
||||
StoredCal s;
|
||||
EEPROM.get(CAL_EEPROM_ADDR, s);
|
||||
if (s.magic != CAL_MAGIC || s.version != CAL_VERSION || s.checksum != checksumOf(s)) return false;
|
||||
memcpy(cal_, s.axes, sizeof(cal_));
|
||||
return true;
|
||||
}
|
||||
|
||||
void Sticks::saveToEeprom() {
|
||||
StoredCal s{};
|
||||
s.magic = CAL_MAGIC;
|
||||
s.version = CAL_VERSION;
|
||||
memcpy(s.axes, cal_, sizeof(cal_));
|
||||
s.checksum = checksumOf(s);
|
||||
EEPROM.put(CAL_EEPROM_ADDR, s);
|
||||
}
|
||||
|
||||
void Sticks::printCalibration(Stream& s) const {
|
||||
static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TWIST", "R_X", "R_Y", "R_TWIST" };
|
||||
s.println("// stick calibration {min, center, max}:");
|
||||
for (int i = 0; i < NUM_AXES; i++) {
|
||||
s.printf("// %-8s { %4d, %4d, %4d }\n", names[i], cal_[i].min, cal_[i].center, cal_[i].max);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
#pragma once
|
||||
#include "Config.h"
|
||||
|
||||
struct AxisCal {
|
||||
int16_t min, center, max; // raw ADC counts
|
||||
};
|
||||
|
||||
// Processed stick values, -1000..1000 per logical axis (see StickAxis):
|
||||
// X + = right, Y + = forward, TWIST + = counterclockwise.
|
||||
struct StickFrame {
|
||||
int16_t v[NUM_AXES] = {};
|
||||
bool allCentered(int16_t tolerance) const;
|
||||
};
|
||||
|
||||
class Sticks {
|
||||
public:
|
||||
// Loads calibration from EEPROM (or Config defaults), then captures the
|
||||
// resting center of each axis. Returns false if a stick looked deflected
|
||||
// at power-on (center capture skipped for that axis).
|
||||
bool begin();
|
||||
void update(); // read + filter + map all axes
|
||||
const StickFrame& frame() const { return frame_; }
|
||||
int16_t raw(int axis) const { return (int16_t)filtered_[axis]; }
|
||||
const AxisCal& cal(int axis) const { return cal_[axis]; }
|
||||
|
||||
// Sweep calibration: start, move every stick to all its limits, release
|
||||
// all sticks, then save. Output is held at zero while calibrating.
|
||||
void startCalibration();
|
||||
bool saveCalibration(); // false = sweep too small, nothing saved
|
||||
void cancelCalibration();
|
||||
bool calibrating() const { return calibrating_; }
|
||||
void resetCalibrationToDefaults(); // also clears EEPROM
|
||||
void printCalibration(Stream& s) const; // paste-able into Config.h
|
||||
|
||||
private:
|
||||
AxisCal cal_[NUM_AXES];
|
||||
AxisCal sweep_[NUM_AXES];
|
||||
float filtered_[NUM_AXES] = {};
|
||||
StickFrame frame_;
|
||||
bool calibrating_ = false;
|
||||
|
||||
int16_t readAveraged(int axis, int samples);
|
||||
int16_t mapAxis(int axis, float raw) const;
|
||||
void applyDefaults();
|
||||
bool loadFromEeprom();
|
||||
void saveToEeprom();
|
||||
};
|
||||
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Operator commands. Produced by the Nextion touchscreen (NextionUI) and by
|
||||
// the USB serial console (SerialConsole) through the same path, so the
|
||||
// remote can be fully exercised on the bench without the display attached.
|
||||
enum class UiCmd : uint8_t {
|
||||
None = 0,
|
||||
ToggleArm,
|
||||
Disarm, // unconditional - the big red button
|
||||
ToggleStand,
|
||||
ToggleGait, // tripod <-> wave
|
||||
ToggleWalkMode, // direct (A) <-> heading-hold (B)
|
||||
ToggleRStickXlate, // right stick: rotation <-> translation
|
||||
ToggleRemoteGyro,
|
||||
ToggleRobotGyro,
|
||||
SetSpeed, // value = 0..255
|
||||
SpeedUp, // serial console only
|
||||
SpeedDown, // serial console only
|
||||
ZeroGyro, // capture the handset's current attitude as "level"
|
||||
CalStartSave, // start sweep calibration, or save it if running
|
||||
CalCancel,
|
||||
};
|
||||
|
||||
struct UiEvent {
|
||||
UiCmd cmd = UiCmd::None;
|
||||
uint8_t value = 0;
|
||||
};
|
||||
|
||||
// Everything the display shows. The remote is authoritative: the Nextion
|
||||
// only sends requests, and always renders what's in here.
|
||||
struct UiView {
|
||||
bool armed = false;
|
||||
bool stand = false;
|
||||
bool gait_wave = false;
|
||||
bool mode_b = false;
|
||||
bool rstick_xlate = false;
|
||||
bool remote_gyro = false;
|
||||
bool robot_gyro = false;
|
||||
bool calibrating = false;
|
||||
uint8_t speed_target = 0;
|
||||
bool link_ok = false;
|
||||
uint8_t link_pct = 0;
|
||||
const char* message = ""; // always a string literal - NextionUI diffs it by pointer
|
||||
};
|
||||
@@ -0,0 +1,117 @@
|
||||
#include "Config.h"
|
||||
#include "Sticks.h"
|
||||
#include "RemoteIMU.h"
|
||||
#include "RemoteState.h"
|
||||
#include "PacketBuilder.h"
|
||||
#include "RadioLink.h"
|
||||
#include "NextionUI.h"
|
||||
#include "SerialConsole.h"
|
||||
|
||||
// Hexapod remote (Teensy 4.1): reads two 3-axis sticks and the handset
|
||||
// MPU6050, takes operator commands from the Nextion touchscreen (and the USB
|
||||
// serial console), and transmits a CmdPacket to the robot at TX_HZ.
|
||||
// See README.md for wiring, calibration and the serial key map.
|
||||
|
||||
Sticks sticks;
|
||||
RemoteIMU imu;
|
||||
RemoteState state;
|
||||
PacketBuilder builder;
|
||||
RadioLink radio_link;
|
||||
NextionUI nextion;
|
||||
SerialConsole console;
|
||||
|
||||
uint32_t last_tx_us = 0;
|
||||
bool dump_packets = false;
|
||||
uint32_t last_dump_ms = 0;
|
||||
CmdPacket last_packet;
|
||||
|
||||
static void printStatus() {
|
||||
static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TW", "R_X", "R_Y", "R_TW" };
|
||||
Serial.println("---- remote status ----");
|
||||
for (int i = 0; i < NUM_AXES; i++)
|
||||
Serial.printf("%-5s raw %4d -> %5d\n", names[i], sticks.raw(i), sticks.frame().v[i]);
|
||||
Serial.printf("IMU %s roll %6.1f pitch %6.1f (deg, vs zeroed level)\n",
|
||||
imu.ok() ? "ok" : "MISSING", imu.rollDeg(), imu.pitchDeg());
|
||||
Serial.printf("radio %s link %s %u%%\n", radio_link.present() ? "ok" : "MISSING",
|
||||
radio_link.linkOk() ? "OK" : "DOWN", radio_link.qualityPct());
|
||||
Serial.printf("armed %d stand %d gait %s mode %s rstick %s remote_gyro %d robot_gyro %d speed %u->%u\n",
|
||||
state.armed(), state.standRequested(), state.gaitWave() ? "wave" : "tripod",
|
||||
state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot",
|
||||
state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent());
|
||||
sticks.printCalibration(Serial);
|
||||
}
|
||||
|
||||
static void dumpPacket(const CmdPacket& p) {
|
||||
Serial.printf("#%3u vx %5d vy %5d yaw %5d | r %5d %5d %5d | tilt %5d %5d | spd %3u flags %02X\n",
|
||||
p.seq, p.vx, p.vy, p.yaw_rate, p.rx, p.ry, p.rz,
|
||||
p.remote_roll, p.remote_pitch, p.speed_scale, p.flags);
|
||||
}
|
||||
|
||||
static void pollInputs(uint32_t now_ms) {
|
||||
UiEvent ev;
|
||||
while (nextion.poll(ev)) state.handle(ev, sticks, imu, now_ms);
|
||||
|
||||
SerialConsole::Local local;
|
||||
while (console.poll(ev, local)) {
|
||||
if (ev.cmd != UiCmd::None) {
|
||||
bool ok = state.handle(ev, sticks, imu, now_ms);
|
||||
if (!ok && state.message(now_ms)[0]) Serial.println(state.message(now_ms));
|
||||
}
|
||||
switch (local) {
|
||||
case SerialConsole::Local::PrintStatus: printStatus(); break;
|
||||
case SerialConsole::Local::ToggleDump: dump_packets = !dump_packets; break;
|
||||
case SerialConsole::Local::ResetCal:
|
||||
if (state.armed()) { Serial.println("disarm first"); break; }
|
||||
sticks.resetCalibrationToDefaults();
|
||||
Serial.println("stick calibration reset to Config.h defaults");
|
||||
break;
|
||||
case SerialConsole::Local::None: break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
nextion.begin();
|
||||
|
||||
// Keep the remote still and both sticks released during power-on: the
|
||||
// IMU captures its gyro bias and the sticks capture their centers here.
|
||||
if (!sticks.begin()) Serial.println("stick center capture skipped on some axes - was a stick held at power-on?");
|
||||
if (!imu.begin()) Serial.println("remote IMU not responding - remote-gyro tilt unavailable");
|
||||
if (!radio_link.begin()) Serial.println("radio not responding - check nRF24 wiring/power");
|
||||
|
||||
state.begin();
|
||||
last_tx_us = micros();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
uint32_t now_ms = millis();
|
||||
|
||||
// UI input is drained every pass, not just on TX ticks, so no touch
|
||||
// events are lost between packets.
|
||||
pollInputs(now_ms);
|
||||
|
||||
uint32_t now_us = micros();
|
||||
float dt = (now_us - last_tx_us) / 1e6f;
|
||||
if (dt < (1.0f / TX_HZ)) return; // hold to the fixed TX rate
|
||||
last_tx_us = now_us;
|
||||
|
||||
sticks.update();
|
||||
imu.update(dt);
|
||||
radio_link.update(now_ms);
|
||||
state.update(dt, radio_link.linkOk(), now_ms);
|
||||
|
||||
last_packet = builder.build(sticks.frame(), imu, state, now_ms);
|
||||
radio_link.send(last_packet);
|
||||
|
||||
UiView view;
|
||||
state.fillView(view, now_ms);
|
||||
view.link_ok = radio_link.linkOk();
|
||||
view.link_pct = radio_link.qualityPct();
|
||||
nextion.render(view, now_ms);
|
||||
|
||||
if (dump_packets && now_ms - last_dump_ms >= 100) {
|
||||
last_dump_ms = now_ms;
|
||||
dumpPacket(last_packet);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user