🌹 Switches to an explicit sense plan act flow

runeharlyk · Nov 14, 2024 · f3d2fec · f3d2fec
1 parent e919b2a
commit f3d2fec
Show file tree

Hide file tree

Showing 5 changed files with 64 additions and 113 deletions.
diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h → esp32/include/spot.h b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h → esp32/include/spot.h
@@ -1,20 +1,5 @@
-#ifndef ESP32SvelteKit_h
-#define ESP32SvelteKit_h
-
-/**
- *   ESP32 SvelteKit
- *
- *   A simple, secure and extensible framework for IoT projects for ESP32 platforms
- *   with responsive Sveltekit front-end built with TailwindCSS and DaisyUI.
- *   https://github.com/theelims/ESP32-sveltekit
- *
- *   Copyright (C) 2018 - 2023 rjwats
- *   Copyright (C) 2023 theelims
- *   Copyright (C) 2024 runeharlyk
- *
- *   All Rights Reserved. This software may be modified and distributed under
- *   the terms of the LGPL v3 license. See the LICENSE file for details.
- **/
+#ifndef Spot_h
+#define Spot_h
 
 #include <Arduino.h>
 
@@ -60,42 +45,36 @@
 #define APPLICATION_CORE -1
 #endif
 
-class ESP32SvelteKit {
+class Spot {
   public:
-    ESP32SvelteKit(PsychicHttpServer *server);
+    Spot(PsychicHttpServer *server);
 
-    void begin();
+    void initialize();
 
-    FS *getFS() { return &ESPFS; }
+    // sense
+    void readSensors() { _peripherals.readIMU(); }
 
-    PsychicHttpServer *getServer() { return _server; }
+    // plan
+    void planMotion() { updatedMotion = _motionService.updateMotion(); }
 
-    EventSocket *getSocket() { return &_socket; }
+    // act
+    void updateActuators() {
+        if (updatedMotion) _servoController.setAngles(_motionService.getAngles());
 
-#if FT_ENABLED(USE_BATTERY)
-    BatteryService *getBatteryService() { return &_batteryService; }
-#endif
-
-#if FT_ENABLED(USE_MOTION)
-    MotionService *getMotionService() { return &_motionService; }
-#endif
-
-#if FT_ENABLED(USE_CAMERA)
-    Camera::CameraService *getCameraService() { return &_cameraService; }
-    Camera::CameraSettingsService *getCameraSettingsService() { return &_cameraSettingsService; }
-#endif
-
-    Peripherals *getPeripherals() { return &_peripherals; }
-
-#if FT_ENABLED(USE_SERVO)
-    ServoController *getServoController() { return &_servoController; }
+        _servoController.updateServoState();
+#if FT_ENABLED(USE_WS2812)
+        _ledService.loop();
 #endif
+    }
 
-    void setMDNSAppName(String name) { _appName = name; }
-
-    void recoveryMode() { _apService.recoveryMode(); }
-
-    void loop();
+    // communicate
+    void emitTelemetry() {
+        if (updatedMotion) EXECUTE_EVERY_N_MS(100, { _motionService.syncAngles(); });
+        // _peripherals.loop();
+        EXECUTE_EVERY_N_MS(1000, { _peripherals.emitIMU(); });
+        // _peripherals.emitSonar();
+        // _peripherals.emitBattery();
+    }
 
   private:
     PsychicHttpServer *_server;
@@ -130,14 +109,16 @@ class ESP32SvelteKit {
     LEDService _ledService;
 #endif
 
-    String _appName = APP_NAME;
+    bool updatedMotion = false;
 
+    String _appName = APP_NAME;
     const u_int16_t _numberEndpoints = 115;
     const u_int32_t _maxFileUpload = 2300000; // 2.3 MB
     const uint16_t _port = 80;
 
   protected:
-    static void _loopImpl(void *_this) { static_cast<ESP32SvelteKit *>(_this)->loop(); }
+    void loop();
+    static void _loopImpl(void *_this) { static_cast<Spot *>(_this)->loop(); }
     void setupServer();
     void setupMDNS();
     void startServices();

diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -2,7 +2,6 @@
 #define MotionService_h
 
 #include <event_socket.h>
-#include <task_manager.h>
 #include <Kinematics.h>
 #include <ServoController.h>
 #include <timing.h>
@@ -37,8 +36,6 @@ class MotionService {
                            std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
 
         body_state.updateFeet(default_feet_positions);
-
-        g_taskManager.createTask(this->_loopImpl, "MotionService", 4096, this, 3);
     }
 
     void anglesEvent(JsonObject &root, int originId) {
@@ -133,16 +130,7 @@ class MotionService {
         return updated;
     }
 
-    void _loop() {
-        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(); }
+    float *getAngles() { return angles; }
 
   private:
     ServoController *_servoController;
@@ -156,15 +144,14 @@ class MotionService {
 
     MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
     unsigned long _lastUpdate;
-    constexpr static int MotionInterval = 15;
 
     body_state_t body_state = {0, 0, 0, 0, 0, 0};
     float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
     float default_feet_positions[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
 
-    float angles[12] = {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};
 };

diff --git a/esp32/lib/ESP32-sveltekit/ServoController.h b/esp32/lib/ESP32-sveltekit/ServoController.h
@@ -7,7 +7,6 @@
 #include <stateful_service.h>
 #include <stateful_service_endpoint.h>
 #include <MathUtils.h>
-#include <timing.h>
 #include <settings/servo_settings.h>
 
 #define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
@@ -73,19 +72,13 @@ class ServoController : public StatefulService<ServoSettings> {
         }
     }
 
-    void loop() {
-        EXECUTE_EVERY_N_MS(ServoInterval, { updateServoState(); });
-    }
-
     StatefulHttpEndpoint<ServoSettings> endpoint;
 
   private:
     Peripherals *_peripherals;
     FSPersistence<ServoSettings> _persistence;
 
     bool is_active {true};
-    constexpr static int ServoInterval = 2;
-
     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};
 };

diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
@@ -1,16 +1,27 @@
-#include <ESP32SvelteKit.h>
+#include <spot.h>
 #include <PsychicHttpServer.h>
 
-#define SERIAL_BAUD_RATE 115200
-
 DRAM_ATTR PsychicHttpServer server;
 
-DRAM_ATTR ESP32SvelteKit spot(&server);
+DRAM_ATTR Spot spot(&server);
+
+void IRAM_ATTR SpotControlLoopEntry(void*) {
+    TickType_t xLastWakeTime = xTaskGetTickCount();
+    for (;;) {
+        spot.readSensors();
+        spot.planMotion();
+        spot.updateActuators();
+        spot.emitTelemetry();
+        vTaskDelayUntil(&xLastWakeTime, 10 / portTICK_PERIOD_MS);
+    }
+}
 
 void setup() {
-    Serial.begin(SERIAL_BAUD_RATE);
+    Serial.begin(115200);
+
+    spot.initialize();
 
-    spot.begin();
+    g_taskManager.createTask(SpotControlLoopEntry, "Spot control task", 4096, nullptr, 3);
 }
 
 void loop() { vTaskDelete(NULL); }
diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp → esp32/src/spot.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp → esp32/src/spot.cpp
@@ -1,21 +1,8 @@
-/**
- *   ESP32 SvelteKit
- *
- *   A simple, secure and extensible framework for IoT projects for ESP32 platforms
- *   with responsive Sveltekit front-end built with TailwindCSS and DaisyUI.
- *   https://github.com/theelims/ESP32-sveltekit
- *
- *   Copyright (C) 2018 - 2023 rjwats
- *   Copyright (C) 2024 theelims
- *   Copyright (C) 2024 runeharlyk
- *
- *   All Rights Reserved. This software may be modified and distributed under
- *   the terms of the LGPL v3 license. See the LICENSE file for details.
- **/
-
-#include <ESP32SvelteKit.h>
-
-ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server)
+#include <spot.h>
+
+static const char *TAG = "Spot";
+
+Spot::Spot(PsychicHttpServer *server)
     :
 #if FT_ENABLED(USE_BATTERY)
       _batteryService(&_peripherals),
@@ -27,9 +14,8 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server)
       _server(server) {
 }
 
-void ESP32SvelteKit::begin() {
-    ESP_LOGV("ESP32SvelteKit", "Loading settings from files system");
-    ESP_LOGI("Running Firmware Version: %s", APP_VERSION);
+void Spot::initialize() {
+    ESP_LOGI(TAG, "Running Firmware Version: %s", APP_VERSION);
     ESPFS.begin(true);
     g_taskManager.begin();
 #if FT_ENABLED(USE_WS2812)
@@ -43,11 +29,11 @@ void ESP32SvelteKit::begin() {
 
     setupMDNS();
 
-    ESP_LOGV("ESP32SvelteKit", "Starting loop task");
-    g_taskManager.createTask(this->_loopImpl, "Spot main", 4096, this, 2, NULL, APPLICATION_CORE);
+    ESP_LOGV(TAG, "Starting misc loop task");
+    g_taskManager.createTask(this->_loopImpl, "Spot misc", 4096, this, 2, NULL, APPLICATION_CORE);
 }
 
-void ESP32SvelteKit::setupServer() {
+void Spot::setupServer() {
     _server->config.max_uri_handlers = _numberEndpoints;
     _server->maxUploadSize = _maxFileUpload;
     _server->listen(_port);
@@ -138,7 +124,7 @@ void ESP32SvelteKit::setupServer() {
 #endif
 
 #ifdef EMBED_WWW
-    ESP_LOGV("ESP32SvelteKit", "Registering routes from PROGMEM static resources");
+    ESP_LOGV(TAG, "Registering routes from PROGMEM static resources");
     WWWData::registerRoutes([&](const String &uri, const String &contentType, const uint8_t *content, size_t len) {
         PsychicHttpRequestCallback requestHandler = [contentType, content, len](PsychicRequest *request) {
             PsychicResponse response(request);
@@ -161,7 +147,7 @@ void ESP32SvelteKit::setupServer() {
     });
 #else
     // Serve static resources from /www/
-    ESP_LOGV("ESP32SvelteKit", "Registering routes from FS /www/ static resources");
+    ESP_LOGV(TAG, "Registering routes from FS /www/ static resources");
     _server->serveStatic("/_app/", ESPFS, "/www/_app/");
     _server->serveStatic("/favicon.png", ESPFS, "/www/favicon.png");
     //  Serving all other get requests with "/www/index.htm"
@@ -179,24 +165,24 @@ void ESP32SvelteKit::setupServer() {
 #endif
 
 #if defined(ENABLE_CORS)
-    ESP_LOGV("ESP32SvelteKit", "Enabling CORS headers");
+    ESP_LOGV(TAG, "Enabling CORS headers");
     DefaultHeaders::Instance().addHeader("Access-Control-Allow-Origin", CORS_ORIGIN);
     DefaultHeaders::Instance().addHeader("Access-Control-Allow-Headers", "Accept, Content-Type, Authorization");
     DefaultHeaders::Instance().addHeader("Access-Control-Allow-Credentials", "true");
 #endif
     DefaultHeaders::Instance().addHeader("Server", _appName);
 }
 
-void ESP32SvelteKit::setupMDNS() {
-    ESP_LOGV("ESP32SvelteKit", "Starting MDNS");
+void Spot::setupMDNS() {
+    ESP_LOGV(TAG, "Starting MDNS");
     MDNS.begin(_wifiService.getHostname());
     MDNS.setInstanceName(_appName);
     MDNS.addService("http", "tcp", 80);
     MDNS.addService("ws", "tcp", 80);
     MDNS.addServiceTxt("http", "tcp", "Firmware Version", APP_VERSION);
 }
 
-void ESP32SvelteKit::startServices() {
+void Spot::startServices() {
     _apService.begin();
 
 #if FT_ENABLED(USE_UPLOAD_FIRMWARE)
@@ -219,20 +205,13 @@ void ESP32SvelteKit::startServices() {
 #endif
 }
 
-void IRAM_ATTR ESP32SvelteKit::loop() {
+void IRAM_ATTR Spot::loop() {
     while (1) {
-#if FT_ENABLED(USE_WS2812)
-        _ledService.loop();
-#endif
         _wifiService.loop();
         _apService.loop();
 #if FT_ENABLED(USE_ANALYTICS)
         _analyticsService.loop();
 #endif
-#if FT_ENABLED(USE_BATTERY)
-        _batteryService.loop();
-#endif
-        _peripherals.loop();
         delay(20);
     }
 }