🚚 Moves firmware to src and include

2025-07-11 12:12:07 +02:00
parent 743aa073b7
commit a3be035f98
60 changed files with 2 additions and 3 deletions
@@ -0,0 +1,137 @@
+#ifndef ServoController_h
+#define ServoController_h
+
+#include <Adafruit_PWMServoDriver.h>
+#include <event_socket.h>
+#include <template/stateful_persistence.h>
+#include <template/stateful_service.h>
+#include <template/stateful_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"
+
+enum class SERVO_CONTROL_STATE { DEACTIVATED, PWM, ANGLE };
+
+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,
+                       [&](JsonVariant &root, int originId) { servoEvent(root, originId); });
+        socket.onEvent(EVENT_SERVO_STATE, [&](JsonVariant &root, int originId) { stateUpdate(root, originId); });
+        _persistence.readFromFS();
+
+        initializePCA();
+        socket.onEvent(EVENT_SERVO_STATE, [&](JsonVariant &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;
+        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 stateUpdate(JsonVariant &root, int originId) {
+        bool active = root["active"].as<bool>();
+        ESP_LOGI("SERVOCONTROLLER", "Setting state %d", active);
+        active ? activate() : deactivate();
+    }
+
+    void servoEvent(JsonVariant &root, int originId) {
+        control_state = SERVO_CONTROL_STATE::PWM;
+        int8_t servo_id = root["servo_id"];
+        uint16_t pwm = root["pwm"].as<uint16_t>();
+        if (servo_id < 0) {
+            uint16_t pwms[12];
+            std::fill_n(pwms, 12, 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 setAngles(float new_angles[12]) {
+        control_state = SERVO_CONTROL_STATE::ANGLE;
+        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.05);
+            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) {
+                continue;
+            }
+            pwms[i] = pwm;
+        }
+        _pca.setMultiplePWM(pwms, 12);
+    }
+
+    void updateServoState() {
+        if (control_state == SERVO_CONTROL_STATE::ANGLE) calculatePWM();
+    }
+
+    StatefulHttpEndpoint<ServoSettings> endpoint;
+
+  private:
+    void initializePCA() {
+        _pca.begin();
+        _pca.setPWMFreq(FACTORY_SERVO_PWM_FREQUENCY);
+        _pca.setOscillatorFrequency(FACTORY_SERVO_OSCILLATOR_FREQUENCY);
+        _pca.sleep();
+    }
+    FSPersistence<ServoSettings> _persistence;
+
+    Adafruit_PWMServoDriver _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};
+};
+
+#endif