#include "ServoDriver.h" #include "Config.h" #include #include // 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(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)); }