Files
hexapod-robot/2026-current/robot/ServoDriver.cpp
T

111 lines
3.9 KiB
C++
Raw Normal View History

#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));
}