28 lines
795 B
C++
28 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));
|
||
|
|
}
|