From 145840edde7fe632ffcc2e80950ff1600af304bc Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 3 Aug 2024 02:49:18 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=A4=20Increases=20motion=20speed?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp |  5 ++---
 esp32/lib/ESP32-sveltekit/MotionService.h    |  3 ++-
 esp32/lib/ESP32-sveltekit/Peripherals.h      | 12 +++++++++++-
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
index 84396e3c..45606a67 100644
--- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
+++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp
@@ -184,6 +184,8 @@ void ESP32SvelteKit::startServices() {
 #endif
     _taskManager.begin();
     _fileExplorer.begin();
+    _peripherals.begin();
+    _servoController.begin();
 #if FT_ENABLED(FT_MOTION)
     _motionService.begin();
 #endif
@@ -191,8 +193,6 @@ void ESP32SvelteKit::startServices() {
     _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
index 553d10a9..38d276a7 100644
--- 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
index 9b47f329..f14325de 100644
--- 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];