2026 remote: tabbed Nextion UI, robot telemetry, servo centre calibration

Protocol v4: CmdPacket grows to 26 bytes (cal_cmd, cal_servo, cal_value);
new 30-byte Telemetry rides back on the nRF24 ack payload (robot state,
IMU, battery, 18 joint angles, packets/s, servo-cal echo). Each end drops
wrong-length packets, so version mismatches still fail closed.

Robot (owner-approved): SERVO_CAL state while disarmed, per-servo centre
trims (+-200 us) in EEPROM, telemetry, battery reading; the IMU estimate
now always runs (leveling still gated by ROBOT GYRO).

Remote: generated HMI (scripts/make_remote_hmi.py) with pages splash,
drive, status, servos, cal, setup, upload; page-aware NextionUI that only
writes the showing page and only what changed; servo calibration and
UPDATE DISPLAY on the touchscreen and serial console; battery module.
The display is on Serial2 (pins 7/8), found on the real remote; the old
sketch's Serial1 was never connected to it.

Sim: RF24 shim supports ack payloads, API version 8
(hxsim_get_ack_payload); sim_smoke checks telemetry and every CAL command.
Unity CmdPacket mirror updated.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed 2026-11-17 14:42:37 +01:00
1 parent 481610c9fd
commit 0b836af8de
40 files changed
+1463 -281

No files matched your search

+10
View File
@@ -0,0 +1,10 @@
#include "Battery.h"
#include "Config.h"
void Battery::update(float dt) {
if (BATTERY_SENSE_PIN == 255) return; // not fitted: stays 0
float v = analogRead(BATTERY_SENSE_PIN) * (BATTERY_ADC_REF_V / BATTERY_ADC_MAX) * BATTERY_DIVIDER;
if (!primed_) { volts_ = v; primed_ = true; return; }
float a = constrain(dt / 2.0f, 0.0f, 1.0f); // ~2 s time constant: servo load makes it sag and bounce
volts_ += (v - volts_) * a;
}
+13
View File
@@ -0,0 +1,13 @@
#pragma once
#include <Arduino.h>
// Battery voltage through a resistor divider (Config.h BATTERY_*), smoothed.
class Battery {
public:
void update(float dt);
uint16_t millivolts() const { return (uint16_t)(volts_ * 1000.0f + 0.5f); } // 0 = not fitted
private:
float volts_ = 0;
bool primed_ = false;
};
+29 -3
View File
@@ -12,22 +12,48 @@ bool Comms::begin() {
SPI.setSCK(RADIO_SCK_PIN); // alternate SCK pin (see Config.h); before RF24 starts SPI
radio_present_ = radio.begin();
if (radio_present_) {
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all
// Telemetry rides back on the auto-ack (ack payloads), which needs dynamic
// payload lengths - update() drops any packet that isn't a CmdPacket.
radio.enableAckPayload();
radio.openReadingPipe(1, HXP_RADIO_ADDRESS);
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.startListening();
loadAckPayload();
}
return radio_present_;
}
// Replaces whatever ack payload is still queued with the latest telemetry, so
// the remote never gets stale data.
void Comms::loadAckPayload() {
radio.flush_tx();
radio.writeAckPayload(1, &telemetry_, sizeof(telemetry_));
}
void Comms::update() {
if (radio_present_) {
bool got = false;
while (radio.available()) {
radio.read(&packet_, sizeof(packet_));
last_rx_ms_ = millis();
uint8_t len = radio.getDynamicPayloadSize();
if (len == sizeof(CmdPacket)) {
radio.read(&packet_, sizeof(packet_));
last_rx_ms_ = millis();
rx_count_++;
got = true;
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
uint8_t junk[32]; // another protocol version: never act on it
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
}
}
if (got) loadAckPayload();
}
uint32_t now = millis();
if (now - rate_window_ms_ >= 1000) {
rx_per_sec_ = (uint8_t)min(rx_count_, (uint16_t)255);
rx_count_ = 0;
rate_window_ms_ = now;
}
link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000);
}
+11
View File
@@ -12,9 +12,20 @@ public:
// Lets the serial test harness (or, later, real radio) inject a packet directly.
void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); }
// Telemetry for the remote. Goes out as the ack payload of the next packet
// received, so set it every loop.
void setTelemetry(const Telemetry& t) { telemetry_ = t; }
uint8_t packetsPerSec() const { return rx_per_sec_; }
private:
CmdPacket packet_;
uint32_t last_rx_ms_ = 0;
bool link_ok_ = false;
bool radio_present_ = false;
Telemetry telemetry_;
uint16_t rx_count_ = 0;
uint8_t rx_per_sec_ = 0;
uint32_t rate_window_ms_ = 0;
void loadAckPayload();
};
+9
View File
@@ -140,6 +140,15 @@ constexpr uint8_t RADIO_CSN_PIN = 15;
constexpr uint8_t RADIO_SCK_PIN = 14;
constexpr float LINK_TIMEOUT_SEC = 0.3f;
// ---- Battery voltage (shown on the remote's STATUS page) ----
// Battery + through a resistor divider into an analog pin. BENCH: no divider
// is wired yet - set the pin and the divider ratio (battery V / pin V) once
// it is. 255 = not fitted: the remote then shows "--".
constexpr uint8_t BATTERY_SENSE_PIN = 255;
constexpr float BATTERY_DIVIDER = 3.0f; // e.g. 20k over 10k
constexpr float BATTERY_ADC_REF_V = 3.3f; // Teensy 3.6 analog reference
constexpr int BATTERY_ADC_MAX = 1023; // default 10-bit analogRead()
// ---- Robot-gyro self-leveling ----
// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
+19 -1
View File
@@ -116,9 +116,27 @@ ignore walk commands.
## Link with the remote
- `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the
exact wire format. Both radios set a static payload size of
exact wire format. `Comms` drops any packet whose length isn't
`sizeof(CmdPacket)`, so a robot and remote built from different protocol
versions won't receive each other at all (and the robot fails safe).
- **Telemetry back to the remote:** after every packet received, `Comms`
loads a fresh `Telemetry` (robot state, IMU roll/pitch, battery, the 18
commanded joint angles, packets per second, servo-cal echo) as the radio's
ack payload. It goes back on the auto-ack of the remote's next packet:
no extra transmissions. `sendTelemetry()` in `robot.ino` fills it every
loop. The IMU attitude is now estimated whether or not robot gyro is on
(the remote shows it); only the leveling is gated by `FLAG_ROBOT_GYRO`.
- **Servo centre calibration** (remote's CAL page): while disarmed, a
`cal_cmd` other than `CAL_NONE` puts the robot in SERVO_CAL. Every servo
is driven to the centre pose (coxa 0, femur 0, tibia −90: legs straight
out, shins straight down) and `CAL_SET` adjusts one servo's centre trim
(µs, ±200, on top of `Config.h` `SERVOS`; it shifts the whole pulse range
like the old `servoCallibration[]`). `CAL_SAVE` stores the 18 trims in
EEPROM (`ServoDriver`, address 0, loaded at power-on), `CAL_REVERT`
reloads them. `CAL_NONE`, arming or link loss relaxes the servos again.
Put the robot on a stand first.
- **Battery:** `Battery` reads `BATTERY_SENSE_PIN` through a divider
(`Config.h`). Not wired yet (pin 255): the remote shows "--".
- Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()`
goes false and the main loop falls back to a zeroed `CmdPacket`, which
disarms the robot.
+1 -1
View File
@@ -38,7 +38,7 @@ bool RobotIMU::readRaw(float& ax, float& ay, float& az, float& gx, float& gy, fl
}
void RobotIMU::update(float dt) {
if (!ok_ || !enabled) return;
if (!ok_) return;
float ax, ay, az, gx, gy, gz;
if (!readRaw(ax, ay, az, gx, gy, gz)) return;
+5 -2
View File
@@ -4,11 +4,14 @@
class RobotIMU {
public:
bool begin(); // returns false if the IMU didn't respond
void update(float dt); // no-op if disabled or the IMU never came up
void update(float dt); // no-op if the IMU never came up
float rollDeg() const { return roll_deg_; }
float pitchDeg() const { return pitch_deg_; }
bool ok() const { return ok_; }
bool enabled = true; // robot-gyro toggle from CmdPacket sets this
// Robot-gyro toggle from CmdPacket. The attitude is estimated either way
// (the remote shows it); robot.ino only levels the body while it's set.
bool enabled = true;
private:
float roll_deg_ = 0, pitch_deg_ = 0;
+71 -2
View File
@@ -1,5 +1,7 @@
#include "ServoDriver.h"
#include "Config.h"
#include <HexapodProtocol.h>
#include <EEPROM.h>
// 18 servos on hardware PWM, the way the old robot sketch drove them (the
// Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per
@@ -28,10 +30,77 @@ void ServoDriver::relaxAll() {
analogWrite(SERVOS[i][j].pin, 0); // no pulses: the servos go limp
}
// ---- Centre trims ----
// EEPROM record at address 0 (nothing else on the robot uses EEPROM). Bump
// TRIM_VERSION if the layout changes - an old record is then ignored.
static constexpr int TRIM_EEPROM_ADDR = 0;
static constexpr uint32_t TRIM_MAGIC = 0x52545848; // "HXTR"
static constexpr uint8_t TRIM_VERSION = 1;
struct TrimRecord {
uint32_t magic;
uint8_t version;
uint8_t checksum; // sum of the trim bytes
int16_t trim_us[NUM_LEGS * 3];
};
static int16_t g_trim[NUM_LEGS * 3] = {};
static bool g_dirty = false;
static bool g_stored = false;
static uint8_t trimChecksum(const int16_t* t) {
uint8_t sum = 0;
const uint8_t* b = reinterpret_cast<const uint8_t*>(t);
for (size_t i = 0; i < sizeof(int16_t) * NUM_LEGS * 3; i++) sum += b[i];
return sum;
}
void ServoDriver::loadTrims() {
TrimRecord r;
EEPROM.get(TRIM_EEPROM_ADDR, r);
g_stored = r.magic == TRIM_MAGIC && r.version == TRIM_VERSION && r.checksum == trimChecksum(r.trim_us);
for (int i = 0; i < NUM_LEGS * 3; i++)
g_trim[i] = g_stored ? constrain(r.trim_us[i], -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US) : 0;
g_dirty = false;
}
static void saveTrims() {
TrimRecord r;
r.magic = TRIM_MAGIC;
r.version = TRIM_VERSION;
memcpy(r.trim_us, g_trim, sizeof(g_trim));
r.checksum = trimChecksum(r.trim_us);
EEPROM.put(TRIM_EEPROM_ADDR, r);
g_dirty = false;
g_stored = true;
}
int16_t ServoDriver::trim(int servo) { return (servo >= 0 && servo < NUM_LEGS * 3) ? g_trim[servo] : 0; }
bool ServoDriver::trimsDirty() { return g_dirty; }
bool ServoDriver::trimsStored() { return g_stored; }
const JointAngles ServoDriver::CAL_POSE = { 0.0f, 0.0f, -90.0f, true };
void ServoDriver::calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us) {
bool valid_servo = servo < NUM_LEGS * 3;
switch (cal_cmd) {
case CAL_SET:
if (valid_servo) {
int16_t v = constrain(value_us, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
if (v != g_trim[servo]) { g_trim[servo] = v; g_dirty = true; }
}
break;
case CAL_SAVE: if (g_dirty) saveTrims(); break; // the remote repeats it: only write once
case CAL_REVERT: if (g_dirty) loadTrims(); break;
default: break; // CAL_HOLD
}
for (int i = 0; i < NUM_LEGS; i++) writeLeg(i, CAL_POSE);
}
int ServoDriver::degToMicros(int leg, int joint, float deg) {
const ServoCal& c = SERVOS[leg][joint];
float us = c.us_at_0deg + c.us_per_deg * deg;
return (int)constrain(us, (float)c.us_min, (float)c.us_max);
int16_t t = g_trim[leg * 3 + joint];
float us = c.us_at_0deg + t + c.us_per_deg * deg;
return (int)constrain(us, (float)(c.us_min + t), (float)(c.us_max + t));
}
void ServoDriver::writeLeg(int i, const JointAngles& a) {
+15 -1
View File
@@ -7,6 +7,20 @@ public:
void relaxAll(); // stop all pulses - call when disarming
void writeLeg(int leg_index, const JointAngles& angles);
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS).
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS)
// plus its centre trim.
static int degToMicros(int leg, int joint, float deg);
// ---- Servo centre calibration (remote's CAL page, CmdPacket.cal_*) ----
// Each servo has a centre trim in us on top of Config.h SERVOS, kept in
// EEPROM. Trims shift the whole pulse range, like the old sketch's
// servoCallibration[].
static void loadTrims(); // from EEPROM; all 0 if none stored. Call in setup()
static int16_t trim(int servo); // servo = leg * 3 + joint
static bool trimsDirty(); // changed since the last load/save
static bool trimsStored(); // the EEPROM holds a trim record
// Applies one calibration command (CAL_* in HexapodProtocol.h) and holds
// every servo at the centre pose. Call every loop while in SERVO_CAL.
void calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us);
static const JointAngles CAL_POSE; // coxa 0, femur 0, tibia -90
};
+46
View File
@@ -9,6 +9,7 @@
#include "Comms.h"
#include "ServoDriver.h"
#include "SmoothedValue.h"
#include "Battery.h"
// Uncomment to drive the robot from the Serial Monitor before a remote exists.
// See README.md for the key map.
@@ -21,6 +22,7 @@ StateMachine state_machine;
InputMapper mapper;
Comms comms;
ServoDriver servo_driver;
Battery battery;
float current_heading_deg = 0; // open-loop: accumulates commanded omega. See note in loop().
SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S); // remote height slider, rate-limited
@@ -28,6 +30,7 @@ float gyro_corr_roll_deg = 0, gyro_corr_pitch_deg = 0; // robot-gyro self-level
float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
uint32_t last_loop_us = 0;
RobotState last_state = RobotState::DISARMED;
bool servo_cal_active = false; // SERVO_CAL: servos held at the centre pose for trimming
#ifdef TEST_SERIAL_INPUT
CmdPacket test_packet;
@@ -63,6 +66,33 @@ void pollSerialTestInput() {
}
#endif
// Robot state for the remote's display, sent back in the radio's ack payload.
void sendTelemetry(const CmdPacket& cmd, const MapperOutput& mapped, RobotState state) {
Telemetry t;
t.roll_ddeg = (int16_t)lroundf(constrain(imu.rollDeg(), -3000.0f, 3000.0f) * 10);
t.pitch_ddeg = (int16_t)lroundf(constrain(imu.pitchDeg(), -3000.0f, 3000.0f) * 10);
t.battery_mv = battery.millivolts();
t.state = servo_cal_active ? TSTATE_SERVO_CAL : (uint8_t)state;
t.status = (imu.ok() ? TSTAT_IMU_OK : 0) |
(mapped.robot_gyro_enabled ? TSTAT_GYRO_ON : 0) |
(state != RobotState::DISARMED || servo_cal_active ? TSTAT_SERVOS_ON : 0) |
(ServoDriver::trimsDirty() ? TSTAT_CAL_DIRTY : 0) |
(ServoDriver::trimsStored() ? TSTAT_TRIMS_STORED : 0);
t.cal_servo = cmd.cal_servo;
t.cal_trim_us = ServoDriver::trim(cmd.cal_servo);
t.rx_per_sec = comms.packetsPerSec();
for (int i = 0; i < NUM_LEGS; i++) {
const float* j = last_joint_deg[i];
float c = servo_cal_active ? ServoDriver::CAL_POSE.coxa_deg : j[0];
float f = servo_cal_active ? ServoDriver::CAL_POSE.femur_deg : j[1];
float b = servo_cal_active ? ServoDriver::CAL_POSE.tibia_deg : j[2];
t.joint_deg[i * 3] = (int8_t)constrain(lroundf(c), -127L, 127L);
t.joint_deg[i * 3 + 1] = (int8_t)constrain(lroundf(f), -127L, 127L);
t.joint_deg[i * 3 + 2] = (int8_t)constrain(lroundf(b) + TELEMETRY_TIBIA_OFFSET_DEG, -127L, 127L);
}
comms.setTelemetry(t);
}
void setup() {
Serial.begin(115200);
gait.begin();
@@ -73,6 +103,7 @@ void setup() {
if (!imu.begin()) Serial.println("robot IMU not responding - stabilization disabled");
if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only");
ServoDriver::loadTrims();
servo_driver.begin();
last_loop_us = micros();
}
@@ -115,6 +146,18 @@ void loop() {
} else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
servo_driver.begin();
}
// Servo centre calibration (remote CAL page): only while disarmed. The
// servos hold the centre pose through their trims; leaving it - CAL_NONE,
// arming or link loss (cmd is then a default packet) - relaxes them again.
bool servo_cal = !mapped.armed && cmd.cal_cmd != CAL_NONE;
if (servo_cal) {
if (!servo_cal_active) servo_driver.begin();
servo_driver.calibrate(cmd.cal_cmd, cmd.cal_servo, cmd.cal_value);
} else if (servo_cal_active && state == RobotState::DISARMED) {
servo_driver.relaxAll();
}
servo_cal_active = servo_cal;
// The stand pose holds every foot at neutral, so walking starts from there.
if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet();
last_state = state;
@@ -203,4 +246,7 @@ void loop() {
servo_driver.writeLeg(i, angles);
}
}
battery.update(dt);
sendTelemetry(cmd, mapped, state);
}