Rune Harlyk
133 lines
4.3 KiB
C++
133 lines
4.3 KiB
C++
#ifndef ServoController_h
|
|
#define ServoController_h
|
|
|
|
#include <peripherals/drivers/pca9685.h>
|
|
#include <eventbus.hpp>
|
|
#include <utils/math_utils.h>
|
|
#include <platform_shared/api.pb.h>
|
|
#include <platform_shared/message.pb.h>
|
|
|
|
#ifndef FACTORY_SERVO_PWM_FREQUENCY
|
|
#define FACTORY_SERVO_PWM_FREQUENCY 50
|
|
#endif
|
|
|
|
#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
|
|
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
|
|
#endif
|
|
|
|
enum class SERVO_CONTROL_STATE { DEACTIVATED, PWM, ANGLE };
|
|
|
|
using ServoSettings = api_ServoSettings;
|
|
|
|
inline ServoSettings ServoSettings_defaults() {
|
|
ServoSettings settings = {};
|
|
settings.servos_count = 12;
|
|
const api_Servo defaults[12] = {
|
|
{306, -1, 0, 2.0f, "Servo1"}, {306, 1, -45, 2.0f, "Servo2"}, {306, 1, 90, 2.0f, "Servo3"},
|
|
{306, -1, 0, 2.0f, "Servo4"}, {306, -1, 45, 2.0f, "Servo5"}, {306, -1, -90, 2.0f, "Servo6"},
|
|
{306, 1, 0, 2.0f, "Servo7"}, {306, 1, -45, 2.0f, "Servo8"}, {306, 1, 90, 2.0f, "Servo9"},
|
|
{306, 1, 0, 2.0f, "Servo10"}, {306, -1, 45, 2.0f, "Servo11"}, {306, -1, -90, 2.0f, "Servo12"}};
|
|
for (int i = 0; i < 12; i++) {
|
|
settings.servos[i] = defaults[i];
|
|
}
|
|
return settings;
|
|
}
|
|
|
|
class ServoController {
|
|
public:
|
|
void begin() {
|
|
_settings = EventBus::instance().peek<ServoSettings>();
|
|
initializePCA();
|
|
|
|
_settingsHandle =
|
|
EventBus::instance().subscribe<ServoSettings>([this](const ServoSettings& s) { _settings = s; });
|
|
servoPwmHandle_ = EventBus::instance().subscribe<socket_message_ServoPWMData>(
|
|
[this](const socket_message_ServoPWMData& data) { setServoPWM(data.servo_id, data.servo_pwm); });
|
|
servoStateHandle_ = EventBus::instance().subscribe<socket_message_ServoStateData>(
|
|
[this](const socket_message_ServoStateData& data) { data.active ? activate() : deactivate(); });
|
|
}
|
|
|
|
void pcaWrite(int index, int value) {
|
|
if (value < 0 || value > 4096) {
|
|
ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
|
|
return;
|
|
}
|
|
_pca.setPWM(index, 0, value);
|
|
}
|
|
|
|
void activate() {
|
|
if (is_active) return;
|
|
control_state = SERVO_CONTROL_STATE::ANGLE;
|
|
is_active = true;
|
|
_pca.wakeup();
|
|
}
|
|
|
|
void deactivate() {
|
|
if (!is_active) return;
|
|
is_active = false;
|
|
control_state = SERVO_CONTROL_STATE::DEACTIVATED;
|
|
_pca.sleep();
|
|
}
|
|
|
|
void setServoPWM(int32_t servo_id, uint32_t pwm) {
|
|
control_state = SERVO_CONTROL_STATE::PWM;
|
|
if (servo_id < 0) {
|
|
uint16_t pwms[12];
|
|
std::fill_n(pwms, 12, static_cast<uint16_t>(pwm));
|
|
_pca.setMultiplePWM(pwms, 12);
|
|
} else {
|
|
_pca.setPWM(servo_id, 0, pwm);
|
|
}
|
|
ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
|
|
}
|
|
|
|
void updateActiveState() { is_active ? activate() : deactivate(); }
|
|
|
|
void setMode(SERVO_CONTROL_STATE newMode) { control_state = newMode; }
|
|
|
|
void setAngles(float new_angles[12]) {
|
|
for (int i = 0; i < 12; i++) {
|
|
target_angles[i] = new_angles[i];
|
|
}
|
|
}
|
|
|
|
void calculatePWM() {
|
|
uint16_t pwms[12];
|
|
for (int i = 0; i < 12; i++) {
|
|
angles[i] = lerp(angles[i], target_angles[i], 0.1);
|
|
auto& servo = _settings.servos[i];
|
|
float angle = servo.direction * angles[i] + servo.center_angle;
|
|
uint16_t pwm = angle * servo.conversion + servo.center_pwm;
|
|
pwms[i] = pwm = std::clamp<uint16_t>(pwm, 125, 600);
|
|
}
|
|
_pca.setMultiplePWM(pwms, 12);
|
|
}
|
|
|
|
void update() {
|
|
if (control_state == SERVO_CONTROL_STATE::ANGLE) calculatePWM();
|
|
}
|
|
|
|
private:
|
|
void initializePCA() {
|
|
_pca.begin();
|
|
_pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
|
|
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
|
|
_pca.sleep();
|
|
}
|
|
|
|
ServoSettings _settings {};
|
|
PCA9685Driver _pca;
|
|
|
|
SERVO_CONTROL_STATE control_state = SERVO_CONTROL_STATE::DEACTIVATED;
|
|
|
|
bool is_active {false};
|
|
float angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
|
|
float target_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
|
|
|
|
SubscriptionHandle _settingsHandle;
|
|
SubscriptionHandle servoPwmHandle_;
|
|
SubscriptionHandle servoStateHandle_;
|
|
};
|
|
|
|
#endif
|