🎉 Connects motion service with servo controller

app/src/lib/stores/model-store.ts

@@ -8,12 +8,14 @@ export const jointNames = persistentStore('joint_names', []);
 
 export const model = writable();
 
-export const modes = ['idle', 'rest', 'stand', 'Crawl', 'walk'] as const;
+export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'crawl', 'walk'] as const;
 
 export type Modes = (typeof modes)[number];
 
 export enum ModesEnum {
+	Deactivated,
 	Idle,
+	Calibration,
 	Rest,
 	Stand,
 	Crawl,

esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp

@@ -59,7 +59,7 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd
       _fileExplorer(server, &_securitySettingsService),
       _servoController(server, &ESPFS, &_securitySettingsService, &_peripherals, &_socket),
 #if FT_ENABLED(FT_MOTION)
-      _motionService(_server, &_socket, &_securitySettingsService, &_taskManager),
+      _motionService(_server, &_socket, &_securitySettingsService, &_servoController, &_taskManager),
 #endif
 #if FT_ENABLED(FT_WS2812)
       _ledService(&_taskManager),
@@ -208,9 +208,6 @@ void IRAM_ATTR ESP32SvelteKit::loop() {
 #if FT_ENABLED(FT_BATTERY)
         _batteryService.loop();
 #endif
-#if FT_ENABLED(FT_MOTION)
-        _motionService.loop();
-#endif
 #if FT_ENABLED(FT_WS2812)
         _ledService.loop();
 #endif

esp32/lib/ESP32-sveltekit/Kinematics.h

@@ -51,13 +51,13 @@ class Kinematics {
     float L, W;
 
     Kinematics() {
-        l1 = 50;
+        l1 = 60.5 / 100;
-        l2 = 20;
+        l2 = 10 / 100;
-        l3 = 120;
+        l3 = 100.7 / 100;
-        l4 = 155;
+        l4 = 118.5 / 100;
 
-        L = 140;
+        L = 207.5 / 100;
-        W = 75;
+        W = 78 / 100;
     }
     ~Kinematics() {}
 
@@ -135,7 +135,8 @@ class Kinematics {
         float H = sqrtf(G * G + z * z);
 
         float theta1 = -atan2f(y, x) - atan2f(F, -l1);
-        float theta3 = acosf((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4));
+        float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4);
+        float theta3 = atan2(sqrt(1 - D * D), D);
         if (isnan(theta3)) theta3 = 0;
         float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3));
         result[0] = RAD_TO_DEG_F(theta1);

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -4,6 +4,7 @@
 #include <EventSocket.h>
 #include <TaskManager.h>
 #include <Kinematics.h>
+#include <ServoController.h>
 #include <Gait/GaitState.h>
 #include <Timing.h>
 #include <MathUtils.h>
@@ -15,13 +16,17 @@
 #define POSITION_EVENT "position"
 #define MODE_EVENT "mode"
 
-enum class MOTION_STATE { IDLE, REST, STAND, WALK };
+enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK };
 
 class MotionService {
   public:
     MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager,
-                  TaskManager *taskManager)
+                  ServoController *servoController, TaskManager *taskManager)
-        : _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager) {}
+        : _server(server),
+          _socket(socket),
+          _securityManager(securityManager),
+          _servoController(servoController),
+          _taskManager(taskManager) {}
 
     void begin() {
         _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
@@ -36,6 +41,8 @@ class MotionService {
                              std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
 
         body_state.updateFeet(default_feet_positions);
+
+        _taskManager->createTask(this->_loopImpl, "MotionService", 4096, this, 3);
     }
 
     void anglesEvent(JsonObject &root, int originId) {
@@ -58,53 +65,60 @@ class MotionService {
 
     void handleInput(JsonObject &root, int originId) {
         JsonArray array = root["data"].as<JsonArray>();
-        float lx = array[1];
+        command.lx = array[1];
-        float ly = array[2];
+        command.lx = array[1];
-        float rx = array[3];
+        command.ly = array[2];
-        float ry = array[4];
+        command.rx = array[3];
-        float h = array[5];
+        command.ry = array[4];
-        float s = array[6];
+        command.h = array[5];
+        command.s = array[6];
 
-        body_state.ym = (h + 128) * (float)0.7;
+        body_state.ym = (command.h + 128.f) * 0.75f / 100;
 
         switch (motionState) {
             case MOTION_STATE::STAND: {
-                body_state.phi = rx / 4;
+                body_state.phi = command.rx / 4;
-                body_state.psi = ry / 4;
+                body_state.psi = command.ry / 4;
-                body_state.xm = ly / 2;
+                body_state.xm = command.ly / 2 / 100;
-                body_state.zm = lx / 2;
+                body_state.zm = command.lx / 2 / 100;
                 break;
             }
         }
     }
 
     void handleMode(JsonObject &root, int originId) {
-        ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>());
         motionState = (MOTION_STATE)root["data"].as<int>();
+        ESP_LOGV("MotionService", "Mode %d", motionState);
         char output[2];
-        sprintf(output, "%d", (int)motionState);
+        itoa((int)motionState, output, 10);
+        motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
         _socket->emit(MODE_EVENT, output, String(originId).c_str());
     }
 
     void syncAngles(const String &originId = "", bool sync = false) {
         char output[100];
-        sprintf(output, "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0], angles[1],
+        snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
-                angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10],
+                 angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
-                angles[11]);
+                 angles[10], angles[11]);
         _socket->emit(ANGLES_EVENT, output, originId.c_str());
+        _servoController->setAngles(angles);
     }
 
     bool updateMotion() {
         switch (motionState) {
-            case MOTION_STATE::IDLE: return false; break;
+            case MOTION_STATE::DEACTIVATED: return false;
-
+            case MOTION_STATE::IDLE: return false;
+            case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break;
             case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
-
+            case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
-            case MOTION_STATE::STAND: {
+            case MOTION_STATE::CRAWL:
+                crawlGait->step(body_state, command);
+                kinematics.calculate_inverse_kinematics(body_state, new_angles);
+                break;
+            case MOTION_STATE::WALK:
+                walkGait->step(body_state, command);
                 kinematics.calculate_inverse_kinematics(body_state, new_angles);
                 break;
-            }
-            case MOTION_STATE::WALK: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
         }
         return update_angles(new_angles, angles);
     }
@@ -121,38 +135,44 @@ class MotionService {
         return updated;
     }
 
-    void loop() {
+    void _loop() {
-        EXECUTE_EVERY_N_MS(MotionInterval, {
+        TickType_t xLastWakeTime = xTaskGetTickCount();
+        for (;;) {
             if (updateMotion()) syncAngles();
-        });
+            _servoController->loop();
+            vTaskDelayUntil(&xLastWakeTime, MotionInterval / portTICK_PERIOD_MS);
         }
+    }
+
+    static void _loopImpl(void *_this) { static_cast<MotionService *>(_this)->_loop(); }
 
   private:
     PsychicHttpServer *_server;
     EventSocket *_socket;
     SecurityManager *_securityManager;
     TaskManager *_taskManager;
+    ServoController *_servoController;
     Kinematics kinematics;
+    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0};
 
-    MOTION_STATE motionState = MOTION_STATE::IDLE;
+    friend class GaitState;
+
+    std::unique_ptr<GaitState> crawlGait = std::make_unique<EightPhaseWalkState>();
+    std::unique_ptr<GaitState> walkGait = std::make_unique<FourPhaseWalkState>();
+
+    MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
     unsigned long _lastUpdate;
-    constexpr static int MotionInterval = 100;
+    constexpr static int MotionInterval = 15;
 
-    body_state_t body_state = {
+    body_state_t body_state = {0, 0, 0, 0, 0, 0};
-        0,
+    float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-    };
-    float new_angles[12] = {
-        0,
-    };
 
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
-    float default_feet_positions[4][4] = {
+    float default_feet_positions[4][4] = {{1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}};
-        {100, -100, 100, 1}, {100, -100, -100, 1}, {-100, -100, 100, 1}, {-100, -100, -100, 1}};
 
-    float angles[12] = {
+    float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-        0,
-    };
     float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 };
 
 #endif

esp32/lib/ESP32-sveltekit/Peripherals.h

@@ -8,8 +8,8 @@
 #include <StatefulService.h>
 #include <MathUtils.h>
 #include <Timing.h>
-#include <list>
 
+#include <list>
 #include <SPI.h>
 #include <Wire.h>
 
@@ -27,9 +27,10 @@
 
 #define EVENT_I2C_SCAN "i2cScan"
 
-#define I2C_INTERVAL 500
+#define I2C_INTERVAL 5000
 #define MAX_ESP_IMU_SIZE 500
 #define EVENT_IMU "imu"
+#define EVENT_SERVO_STATE "servoState"
 
 /*
  * Servo Settings
@@ -116,6 +117,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 #endif
           _fsPersistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, fs,
                          DEVICE_CONFIG_FILE) {
+        _accessMutex = xSemaphoreCreateMutex();
         addUpdateHandler([&](const String &originId) { updatePins(); }, false);
     };
 
@@ -140,6 +142,11 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         _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) {
+            _pca_active = root["active"] | false;
+            _pca_active ? pcaActivate() : pcaDeactivate();
+        });
 #endif
 
 #if FT_ENABLED(FT_IMU)
@@ -182,9 +189,11 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     void loop() {
         EXECUTE_EVERY_N_MS(_updateInterval, {
+            beginTransaction();
             updateImu();
             readSonar();
             emitSonar();
+            endTransaction();
         });
     }
 
@@ -231,16 +240,53 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     }
 
     /* SERVO FUNCTIONS*/
-    void pcaWrite(int index, int value) {
+    void pcaLerpTo(int index, int value, float t) {
 #if FT_ENABLED(FT_SERVO)
-        _pca.setPWM(index, 0, value);
+        target_pwm[index] = lerp(pwm[index], value, t);
 #endif
     }
 
-    void pcaAngle(int index, int value) {
+    void pcaWrite(int index, int value) {
 #if FT_ENABLED(FT_SERVO)
-        // uint16_t pwm = SERVO_MIN + value * SERVO_ANGLE_PWM_RATIO;
+        if (value < 0 || value > 4096) {
-        _pca.setPWM(index, 0, 125 + value * 2);
+            ESP_LOGE("Peripherals", "Invalid PWM value %d for %d :: Valid range 0-4096", value, index);
+            return;
+        }
+        pwm[index] = value;
+        target_pwm[index] = value;
+        beginTransaction();
+        _pca.setPWM(index, 0, value);
+        endTransaction();
+#endif
+    }
+
+    void pcaWriteAngle(int index, int angle) {
+#if FT_ENABLED(FT_SERVO)
+        _pca.setPWM(index, 0, 125 + angle * 2);
+#endif
+    }
+
+    void pcaWriteAngles(float *angles, int numServos, int offset = 0) {
+#if FT_ENABLED(FT_SERVO)
+        for (int i = 0; i < numServos; i++) {
+            pcaWriteAngle(i + offset, angles[i]);
+        }
+#endif
+    }
+
+    void pcaActivate() {
+#if FT_ENABLED(FT_SERVO)
+        if (_pca_active) return;
+        _pca_active = true;
+        _pca.wakeup();
+#endif
+    }
+
+    void pcaDeactivate() {
+#if FT_ENABLED(FT_SERVO)
+        if (!_pca_active) return;
+        _pca_active = false;
+        _pca.sleep();
 #endif
     }
 
@@ -405,11 +451,19 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
     FSPersistence<PeripheralsConfiguration> _fsPersistence;
 
+    SemaphoreHandle_t _accessMutex;
+    inline void beginTransaction() { xSemaphoreTakeRecursive(_accessMutex, portMAX_DELAY); }
+
+    inline void endTransaction() { xSemaphoreGiveRecursive(_accessMutex); }
+
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
 
 #if FT_ENABLED(FT_SERVO)
     Adafruit_PWMServoDriver _pca;
+    bool _pca_active {false};
+    uint16_t pwm[16] = {0};
+    uint16_t target_pwm[16] = {0};
 #endif
 #if FT_ENABLED(FT_IMU)
     MPU6050 _imu;