♨️ Adds test for gait performance

esp32/platformio.ini

@@ -96,8 +96,18 @@ lib_deps =
 	adafruit/Adafruit HMC5883 Unified@^1.2.3
 	adafruit/Adafruit BMP085 Unified@^1.1.3
 	adafruit/Adafruit ADS1X15@^2.5.0
+	adafruit/Adafruit Unified Sensor@^1.1.14
 	fastled/FastLED@^3.7.0
     SPI
+	FS
+	WiFi
+	LittleFS
+	ESPmDNS
+	HTTPClient
+	DNSServer
+	Update
+	WiFiClientSecure
+	HTTPUpdate
 extra_scripts = 
 	pre:scripts/pre_build.py
     pre:scripts/build_app.py

esp32/test/gait_performance.cpp

@@ -0,0 +1,33 @@
+#include <Arduino.h>
+#include <unity.h>
+#include "Gait/GaitState.h"
+
+void test_gaitPlanner_calculateStep_time() {
+    BezierState gaitPlanner;
+    body_state_t body_state = {
+        128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
+    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
+    const int num_steps = 1000;
+
+    unsigned long start = millis();
+    for (int i = 0; i < num_steps; i++) {
+        gaitPlanner.step(body_state, command, 0.02f);
+    }
+    unsigned long end = millis();
+
+    unsigned long duration = end - start;
+    unsigned long max_duration = num_steps / 2; // Minimum 0.5 ms per step
+
+    char message[50];
+    snprintf(message, sizeof(message), "The step calculation took: %lu ms", duration);
+    TEST_ASSERT_MESSAGE(duration <= max_duration, message);
+}
+
+void setup() {
+    delay(2000); // Allow time for Serial to initialize if running on an ESP32
+    UNITY_BEGIN();
+    RUN_TEST(test_gaitPlanner_calculateStep_time);
+    UNITY_END();
+}
+
+void loop() {}