mangavalkandClaude Opus 5.5
5ff85c4eb8
- LEGACY/Hexapod_Teensy/: old robot sketch (reference only). - platformio.ini: robot env targets teensy36 (owner-confirmed board). - Config.h: per-servo SERVOS table (pin, pulse at 0 deg, signed us/deg, pulse limits) built from the old sketch's pins, ranges, trims and the old Unity servo directions. Rear-left/front-left coxa offsets set to 0: the owner re-horns those two servos (the old +-90 left them at the end of travel; the sim showed them clamping). - ServoDriver: 18 servos on hardware PWM (50 Hz, 16-bit); the Teensy 3.6 Servo library stops at 12. - Radio on CE 39 / CSN 15 with SPI SCK on pin 14. - sim: analogWrite/PWM shims, pulses decoded through SERVOS (API v6). - Docs: CLAUDE.md, READMEs. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
43 lines
1.5 KiB
C++
43 lines
1.5 KiB
C++
#include "ServoDriver.h"
|
|
#include "Config.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
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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));
|
|
}
|