🚤 Increases motion speed

runeharlyk · Aug 3, 2024 · 145840e · 145840e
1 parent bebdc51
commit 145840e
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 5 deletions.
diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
@@ -184,15 +184,15 @@ void ESP32SvelteKit::startServices() {
 #endif
     _taskManager.begin();
     _fileExplorer.begin();
+    _peripherals.begin();
+    _servoController.begin();
 #if FT_ENABLED(FT_MOTION)
     _motionService.begin();
 #endif
 #if FT_ENABLED(FT_CAMERA)
     _cameraService.begin();
     _cameraSettingsService.begin();
 #endif
-    _peripherals.begin();
-    _servoController.begin();
 #if FT_ENABLED(FT_WS2812)
     _ledService.begin();
 #endif
@@ -208,7 +208,6 @@ void IRAM_ATTR ESP32SvelteKit::loop() {
 #if FT_ENABLED(FT_BATTERY)
         _batteryService.loop();
 #endif
-        _servoController.loop();
 #if FT_ENABLED(FT_WS2812)
         _ledService.loop();
 #endif

diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -139,6 +139,7 @@ class MotionService {
         TickType_t xLastWakeTime = xTaskGetTickCount();
         for (;;) {
             if (updateMotion()) syncAngles();
+            _servoController->loop();
             vTaskDelayUntil(&xLastWakeTime, MotionInterval / portTICK_PERIOD_MS);
         }
     }
@@ -161,7 +162,7 @@ class MotionService {
 
     MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
     unsigned long _lastUpdate;
-    constexpr static int MotionInterval = 20;
+    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};

diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h
@@ -27,7 +27,7 @@
 
 #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"
@@ -117,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);
     };
 
@@ -188,10 +189,12 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     void loop() {
         EXECUTE_EVERY_N_MS(_updateInterval, {
+            beginTransaction();
             readIMU();
             updateImu();
             readSonar();
             emitSonar();
+            endTransaction();
         });
     }
 
@@ -252,7 +255,9 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         }
         pwm[index] = value;
         target_pwm[index] = value;
+        beginTransaction();
         _pca.setPWM(index, 0, value);
+        endTransaction();
 #endif
     }
 
@@ -447,6 +452,11 @@ 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];