mangavalkandClaude Opus 5.5
ad77b06c36
Robot-side firmware (Teensy 4.0) imported unchanged as extracted. platformio.ini defines [env:robot] (teensy40) and [env:remote] (teensy41); scripts/select_src_dir.py points each env at its own sketch folder so robot/ and remote/ stay Arduino-style sketches. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
29 lines
795 B
C++
29 lines
795 B
C++
#include "ServoDriver.h"
|
|
#include "Config.h"
|
|
#include <Servo.h>
|
|
|
|
static Servo servos[NUM_LEGS][3];
|
|
|
|
void ServoDriver::begin() {
|
|
for (int i = 0; i < NUM_LEGS; i++)
|
|
for (int j = 0; j < 3; j++)
|
|
servos[i][j].attach(SERVO_PINS[i][j]);
|
|
}
|
|
|
|
void ServoDriver::relaxAll() {
|
|
for (int i = 0; i < NUM_LEGS; i++)
|
|
for (int j = 0; j < 3; j++)
|
|
servos[i][j].detach();
|
|
}
|
|
|
|
static int degToMicros(float deg) {
|
|
return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG);
|
|
}
|
|
|
|
void ServoDriver::writeLeg(int i, const JointAngles& a) {
|
|
if (!a.valid) return; // never write an out-of-range or unreachable solution
|
|
servos[i][0].writeMicroseconds(degToMicros(a.coxa_deg));
|
|
servos[i][1].writeMicroseconds(degToMicros(a.femur_deg));
|
|
servos[i][2].writeMicroseconds(degToMicros(a.tibia_deg));
|
|
}
|