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>
112 lines
3.9 KiB
C++
112 lines
3.9 KiB
C++
#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
|
|
// step. Every servo pin must be PWM-capable - on the Teensy 3.6 all of the
|
|
// SERVOS[] pins are.
|
|
static constexpr uint8_t PWM_BITS = 16;
|
|
static constexpr float PERIOD_US = 1e6f / SERVO_PWM_HZ;
|
|
|
|
static void writePulse(uint8_t pin, float us) {
|
|
analogWrite(pin, (int)(us / PERIOD_US * (1 << PWM_BITS) + 0.5f));
|
|
}
|
|
|
|
void ServoDriver::begin() {
|
|
analogWriteResolution(PWM_BITS);
|
|
for (int i = 0; i < NUM_LEGS; i++)
|
|
for (int j = 0; j < 3; j++) {
|
|
analogWriteFrequency(SERVOS[i][j].pin, SERVO_PWM_HZ);
|
|
pinMode(SERVOS[i][j].pin, OUTPUT);
|
|
analogWrite(SERVOS[i][j].pin, 0); // no pulses until the first writeLeg()
|
|
}
|
|
}
|
|
|
|
void ServoDriver::relaxAll() {
|
|
for (int i = 0; i < NUM_LEGS; i++)
|
|
for (int j = 0; j < 3; j++)
|
|
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];
|
|
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) {
|
|
if (!a.valid) return; // never write an out-of-range or unreachable solution
|
|
writePulse(SERVOS[i][0].pin, degToMicros(i, 0, a.coxa_deg));
|
|
writePulse(SERVOS[i][1].pin, degToMicros(i, 1, a.femur_deg));
|
|
writePulse(SERVOS[i][2].pin, degToMicros(i, 2, a.tibia_deg));
|
|
}
|