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:
1 parent
481610c9fd
commit
0b836af8de
40 files changed
+1463
-281
No files matched your search
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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();
|
||||
};
|
||||
@@ -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.
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
};
|
||||
@@ -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);
|
||||
}
|
||||
Reference in new issue
Block a user