Rune Harlyk
123 lines
3.9 KiB
C++
123 lines
3.9 KiB
C++
#ifndef ServoController_h
|
|
#define ServoController_h
|
|
|
|
#include <Adafruit_PWMServoDriver.h>
|
|
#include <event_socket.h>
|
|
#include <stateful_persistence.h>
|
|
#include <stateful_service.h>
|
|
#include <stateful_service_endpoint.h>
|
|
#include <utils/math_utils.h>
|
|
#include <settings/servo_settings.h>
|
|
|
|
/*
|
|
* Servo Settings
|
|
*/
|
|
#ifndef FACTORY_SERVO_PWM_FREQUENCY
|
|
#define FACTORY_SERVO_PWM_FREQUENCY 50
|
|
#endif
|
|
|
|
#ifndef FACTORY_SERVO_OSCILLATOR_FREQUENCY
|
|
#define FACTORY_SERVO_OSCILLATOR_FREQUENCY 27000000
|
|
#endif
|
|
|
|
#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
|
|
#define EVENT_SERVO_STATE "servoState"
|
|
|
|
class ServoController : public StatefulService<ServoSettings> {
|
|
public:
|
|
ServoController()
|
|
: endpoint(ServoSettings::read, ServoSettings::update, this),
|
|
_persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
|
|
|
|
void begin() {
|
|
socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
|
|
[&](JsonObject &root, int originId) { servoEvent(root, originId); });
|
|
socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
|
|
_persistence.readFromFS();
|
|
|
|
_pca.begin();
|
|
_pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
|
|
_pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
|
|
_pca.sleep();
|
|
socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
|
|
is_active = root["active"] | false;
|
|
is_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;
|
|
is_active = true;
|
|
_pca.wakeup();
|
|
}
|
|
|
|
void deactivate() {
|
|
if (!is_active) return;
|
|
is_active = false;
|
|
_pca.sleep();
|
|
}
|
|
|
|
void stateUpdate(JsonObject &root, int originId) {
|
|
bool active = root["active"].as<bool>();
|
|
ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
|
|
active ? activate() : deactivate();
|
|
}
|
|
|
|
void servoEvent(JsonObject &root, int originId) {
|
|
uint8_t servo_id = root["servo_id"];
|
|
uint16_t pwm = root["pwm"];
|
|
ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
|
|
}
|
|
|
|
void syncAngles(const String &originId) {
|
|
char output[100];
|
|
snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
|
|
angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
|
|
angles[10], angles[11]);
|
|
socket.emit("angles", output, String(originId).c_str());
|
|
}
|
|
|
|
void updateActiveState() { is_active ? activate() : deactivate(); }
|
|
|
|
void setAngles(float new_angles[12]) {
|
|
for (int i = 0; i < 12; i++) {
|
|
target_angles[i] = new_angles[i];
|
|
}
|
|
}
|
|
|
|
void updateServoState() {
|
|
for (int i = 0; i < 12; i++) {
|
|
angles[i] = lerp(angles[i], target_angles[i], 0.2);
|
|
auto &servo = _state.servos[i];
|
|
float angle = servo.direction * angles[i] + servo.centerAngle;
|
|
uint16_t pwm = angle * servo.conversion + servo.centerPwm;
|
|
if (pwm < 125 || pwm > 600) {
|
|
ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
|
|
continue;
|
|
}
|
|
pcaWrite(i, pwm);
|
|
}
|
|
}
|
|
|
|
StatefulHttpEndpoint<ServoSettings> endpoint;
|
|
|
|
private:
|
|
FSPersistence<ServoSettings> _persistence;
|
|
|
|
Adafruit_PWMServoDriver _pca;
|
|
uint16_t pwm[16] = {0};
|
|
|
|
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};
|
|
};
|
|
|
|
#endif |