🪄 Major refactoring to use ESP32 template

This includes the change to use https://github.com/runeharlyk/ESP32-rapid-development-template.git as a freeRTos template
2023-08-02 22:26:09 +02:00
parent 4c7b8954eb
commit 4282055be0
42 changed files with 2169 additions and 1065 deletions
@@ -1,16 +1,114 @@
-#ifndef SERVO_H
-#define SERVO_H
+#pragma once

-#include <esp_err.h>
+#include <memory>
+#include <Adafruit_PWMServoDriver.h>
+#include <globals.h>
+#include <webserver.h>

-typedef struct {
-  uint16_t pulse_0; 
-  uint16_t pulse_180; 
-  int8_t invert;
-} servo_settings_t;
+#if USE_WIFI && USE_WEBSERVER
+    extern DRAM_ATTR CWebServer g_WebServer;
+#endif

-esp_err_t disable_servos();
-esp_err_t setup_pwm_controller();
-esp_err_t set_servo(uint8_t id, uint16_t angle);
+typedef  struct {
+    float omega;
+    float phi;
+    float psi;
+    float xm;
+    float ym;
+    float zm;
+    bool set;
+} position_t;

-#endif
+class Servo : public Adafruit_PWMServoDriver {
+  public:
+    
+    Servo() : Adafruit_PWMServoDriver() {}
+
+    void SetAngles(int8_t* angle) {
+        for(size_t i = 0; i < 12; i++)
+            servo_angles[i] = angle[i];
+        updateServos();
+    }
+
+    void SetAngle(uint8_t id, int8_t angle) {
+        servo_angles[id] = angle;
+        updateServos();
+    }
+
+    void updateServos() {
+        for(uint8_t i = 0; i < 12; i++){
+            int8_t angle = servo_angles[i];
+            uint16_t pulse = (uint16_t) (0.5 + servo_min[i] + (((angle * servo_invert[i]) + 90) * servo_conversion[i]));
+            setPWM(i, 0, pulse);
+        }
+        broadcastAngles();
+    }
+
+    void setBody(float phi, float theta, float psi, float x, float y, float z) {
+        goal_position.phi = (phi - 128) / 2;
+        goal_position.omega = (theta - 128) / 2;
+        goal_position.psi = (psi - 128) / 2; 
+        goal_position.xm = (x - 128) / 2;
+        goal_position.ym = (y - 128) / 2;
+        goal_position.zm = (z - 128) / 2;
+        updateAngles();
+    }
+
+    void setBodyAngle(float phi, float theta, float psi) {
+        goal_position.phi = phi;
+        goal_position.omega = theta;
+        goal_position.psi = psi;
+        updateAngles();
+    }
+
+    void setBodyPosition(float x, float y, float z) {
+        goal_position.xm = x;
+        goal_position.ym = y;
+        goal_position.zm = z;
+        updateAngles();
+    }
+
+    void updateAngles() {
+        servo_angles[0] = goal_position.phi;
+        servo_angles[1] = goal_position.omega;
+        servo_angles[2] = goal_position.psi;
+        servo_angles[3] = goal_position.xm;
+        servo_angles[4] = goal_position.ym;
+        servo_angles[5] = goal_position.zm;
+        updateServos();
+        broadcastAngles();
+    }
+
+    void deactivate() {
+        isActive = false;
+        sleep();
+    }
+
+    void activate() {
+        isActive = true;
+        sleep();
+    }
+
+    void toggleState() {
+        isActive ? sleep() : wakeup();
+        isActive = !isActive;
+    }
+
+    bool isActive {true};
+
+  private:
+    void broadcastAngles() {
+        uint8_t* buf = (uint8_t*)&servo_angles;
+        g_WebServer.broadcast(buf, 12);
+    }
+
+    const int16_t servo_min[12] {92,101,129,92,118,125,110,101,125,92,101,125};
+    const int8_t servo_invert[12] = {-1,1,1, -1,-1,-1, 1,1,1, 1,-1,-1};
+    const float servo_conversion[12] {2.2,2.1055555,1.96923,2.2,2.1055555,1.96923,2.2,2.1055555,1.96923,2.2,2.1055555,1.96923};
+
+    position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
+    position_t goal_position = {0,};
+    int8_t servo_angles[12]{0,};
+};
+
+extern DRAM_ATTR std::unique_ptr<Servo> g_ptrServo;