🦾 Adds kinematics

app/src/routes/controller/Controls.svelte

@@ -68,7 +68,8 @@
 	};
 
     const handleRange = (event:Event, key: 'speed' | 'height') => {
-        const value:number = event.detail
+        const value:number = event.target?.value
+        
         input.update((inputData) => {
 			inputData[key] = value;
 			return inputData;

esp32/.vscode/c_cpp_properties.json

@@ -8,23 +8,35 @@
         {
             "name": "PlatformIO",
             "includePath": [
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/include",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/include",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/ESP32-sveltekit",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/ESP32-sveltekit",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_HMC5883_Unified-1.2.3",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_BMP085_Unified-1.1.3",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_Unified_Sensor-1.1.14",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Update/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/PsychicHttp/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/PsychicHttp/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/FS/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/UrlEncode/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/UrlEncode/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/ArduinoTrace",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoTrace",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/PsychicMqttClient/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit PWM Servo Driver Library",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit SSD1306",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit GFX Library",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit BusIO",
+                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/I2Cdevlib-MPU6050",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/I2Cdevlib-Core",
+                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/NewPing/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/PsychicMqttClient/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson/src",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos",
@@ -183,13 +195,11 @@
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo",
-                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/src/include",
-                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/esp-tts/esp_tts_chinese/include",
-                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/include/esp32",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include",
+                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include",
@@ -217,8 +227,8 @@
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/cores/esp32",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/variants/esp32",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/spot_micro_kinematcs_cpp/include",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson@6.21.5/src",
-                "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/spot_micro_kinematcs_cpp",
+                "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/MPU6050_light/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src",
@@ -235,36 +245,46 @@
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SD/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src",
-                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/USB/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src",
                 "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src",
-                "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src",
                 ""
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": true,
                 "path": [
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/include",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/include",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/ESP32-sveltekit",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/ESP32-sveltekit",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_HMC5883_Unified-1.2.3",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_BMP085_Unified-1.1.3",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/Adafruit_Unified_Sensor-1.1.14",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/LittleFS/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/HTTPUpdate/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Update/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/HTTPClient/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/DNSServer/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/PsychicHttp/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/lib/PsychicHttp/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/FS/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/UrlEncode/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/UrlEncode/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/ArduinoTrace",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoTrace",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/ESPmDNS/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/PsychicMqttClient/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit PWM Servo Driver Library",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit SSD1306",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit GFX Library",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/Adafruit BusIO",
+                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/I2Cdevlib-MPU6050",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/I2Cdevlib-Core",
+                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/NewPing/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/PsychicMqttClient/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFiClientSecure/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson/src",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/newlib/platform_include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/freertos/include/esp_additions/freertos",
@@ -423,13 +443,11 @@
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/layer",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/detect",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-dl/include/model_zoo",
-                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/src/include",
-                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/esp-tts/esp_tts_chinese/include",
-                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp-sr/include/esp32",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/driver/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/esp32-camera/conversions/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/dotprod/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/include",
+                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/support/mem/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/hann/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/include/espressif__esp-dsp/modules/windows/blackman/include",
@@ -457,8 +475,8 @@
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/tools/sdk/esp32/dio_qspi/include",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/cores/esp32",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/variants/esp32",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/spot_micro_kinematcs_cpp/include",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/ArduinoJson@6.21.5/src",
-                    "C:/data/repos/Hardware/Spot Micro - Leika/esp32/lib/spot_micro_kinematcs_cpp",
+                    "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/libdeps/esp32dev/MPU6050_light/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/ArduinoOTA/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/AsyncUDP/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/BLE/src",
@@ -475,28 +493,26 @@
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/RainMaker/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SD/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SD_MMC/src",
-                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SPIFFS/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/SimpleBLE/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Ticker/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/USB/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WebServer/src",
                     "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/WiFiProv/src",
-                    "C:/Users/Rune/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src",
                     ""
                 ]
             },
             "defines": [
-                "PLATFORMIO=60114",
+                "PLATFORMIO=60115",
                 "ARDUINO_ESP32_DEV",
                 "APP_NAME=\"Spot-Micro\"",
                 "APP_VERSION=\"0.0.1\"",
                 "FACTORY_WIFI_SSID=\"\"",
                 "FACTORY_WIFI_PASSWORD=\"\"",
-                "FACTORY_WIFI_HOSTNAME=\"#{platform}-#{unique_id}\"",
+                "FACTORY_WIFI_HOSTNAME=\"spot-micro\"",
                 "FACTORY_AP_PROVISION_MODE=AP_MODE_DISCONNECTED",
                 "FACTORY_AP_SSID=\"Spot-Micro-#{unique_id}\"",
-                "FACTORY_AP_PASSWORD=\"esp-react\"",
+                "FACTORY_AP_PASSWORD=\"spot-leika\"",
                 "FACTORY_AP_CHANNEL=1",
                 "FACTORY_AP_SSID_HIDDEN=false",
                 "FACTORY_AP_MAX_CLIENTS=4",
@@ -526,18 +542,24 @@
                 "FACTORY_JWT_SECRET=\"#{random}-#{random}\"",
                 "FT_BATTERY=0",
                 "FT_NTP=1",
-                "FT_SECURITY=1",
+                "FT_SECURITY=0",
                 "FT_MQTT=0",
                 "FT_SLEEP=0",
-                "FT_UPLOAD_FIRMWARE=1",
+                "FT_UPLOAD_FIRMWARE=0",
-                "FT_DOWNLOAD_FIRMWARE=1",
+                "FT_DOWNLOAD_FIRMWARE=0",
                 "FT_ANALYTICS=1",
+                "FT_IMU=1",
+                "FT_MAG=1",
+                "FT_BMP=1",
+                "FT_GPS=1",
                 "BUILD_TARGET=\"esp32dev\"",
                 "ESP32SVELTEKIT_RUNNING_CORE=0",
                 "EMBED_WWW",
-                "ENABLE_CORS",
+                "SERVE_CONFIG_FILES",
                 "CORS_ORIGIN=\"*\"",
-                "CORE_DEBUG_LEVEL=5",
+                "ENABLE_CORS",
+                "CORE_DEBUG_LEVEL=3",
+                "register=",
                 "LED_BUILTIN=2",
                 "KEY_BUILTIN=0",
                 "HAVE_CONFIG_H",
@@ -545,7 +567,7 @@
                 "UNITY_INCLUDE_CONFIG_H",
                 "WITH_POSIX",
                 "_GNU_SOURCE",
-                "IDF_VER=\"v4.4.4\"",
+                "IDF_VER=\"v4.4.6-dirty\"",
                 "ESP_PLATFORM",
                 "_POSIX_READER_WRITER_LOCKS",
                 "ARDUINO_ARCH_ESP32",
@@ -559,7 +581,7 @@
             ],
             "cStandard": "gnu99",
             "cppStandard": "gnu++17",
-            "compilerPath": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32/bin/xtensa-esp32-elf-gcc.exe",
+            "compilerPath": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32@8.4.0+2021r2-patch5/bin/xtensa-esp32-elf-gcc.exe",
             "compilerArgs": [
                 "-mlongcalls",
                 ""

esp32/.vscode/extensions.json

@@ -1,7 +1,10 @@
 {
     // See http://go.microsoft.com/fwlink/?LinkId=827846
     // for the documentation about the extensions.json format
-    "recommendations": ["platformio.platformio-ide", "esbenp.prettier-vscode"],
+    "recommendations": [
+        "esbenp.prettier-vscode",
+        "platformio.platformio-ide"
+    ],
     "unwantedRecommendations": [
         "ms-vscode.cpptools-extension-pack"
     ]

esp32/.vscode/launch.json

@@ -12,9 +12,9 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug",
-            "executable": "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/build/esp32dev/firmware.elf",
+            "executable": "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/build/esp32dev/firmware.elf",
             "projectEnvName": "esp32dev",
-            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32/bin",
+            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32@8.4.0+2021r2-patch5/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "preLaunchTask": {
                 "type": "PlatformIO",
@@ -25,18 +25,18 @@
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (skip Pre-Debug)",
-            "executable": "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/build/esp32dev/firmware.elf",
+            "executable": "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/build/esp32dev/firmware.elf",
             "projectEnvName": "esp32dev",
-            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32/bin",
+            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32@8.4.0+2021r2-patch5/bin",
             "internalConsoleOptions": "openOnSessionStart"
         },
         {
             "type": "platformio-debug",
             "request": "launch",
             "name": "PIO Debug (without uploading)",
-            "executable": "C:/data/repos/Hardware/Spot Micro - Leika/esp32/.pio/build/esp32dev/firmware.elf",
+            "executable": "C:/data/repos/Hardware/Spot_Micro_Leika/esp32/.pio/build/esp32dev/firmware.elf",
             "projectEnvName": "esp32dev",
-            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32/bin",
+            "toolchainBinDir": "C:/Users/Rune/.platformio/packages/toolchain-xtensa-esp32@8.4.0+2021r2-patch5/bin",
             "internalConsoleOptions": "openOnSessionStart",
             "loadMode": "manual"
         }

esp32/build/firmware/Spot-Micro_esp32cam_0-0-1.md5

@@ -0,0 +1 @@
+e26accd60ea0ebc13e6ab02a735aa5ee

esp32/lib/ESP32-sveltekit/Kinematics.h

@@ -0,0 +1,171 @@
+#ifndef Kinematics_h
+#define Kinematics_h
+
+#include <cmath>
+#include <esp_dsp.h>
+
+typedef  struct {
+    float omega;
+    float phi;
+    float psi;
+    float xm;
+    float ym;
+    float zm;
+    bool set;
+} position_t;
+
+
+#define RAD2DEGREES 57.295779513082321 // 180 / PI
+#define DEGREES2RAD 0.017453292519943
+
+class Kinematics
+{
+private:
+    dspm::Mat Tlf;
+    dspm::Mat Trf;
+    dspm::Mat Tlb;
+    dspm::Mat Trb;
+
+    float inverse[4][4];
+
+    dspm::Mat Ix;
+
+public:
+    float l1, l2, l3, l4;
+    float L, W;
+    Kinematics(){
+        l1 = 50;
+        l2 = 20;
+        l3 = 120;
+        l4 = 155;
+
+        L = 140;
+        W = 75;
+
+        float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
+        dspm::Mat Ix(Ix_data, 4, 4);
+    }
+    ~Kinematics(){}
+
+    esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) {
+        
+        esp_err_t res = bodyIK(p);
+
+        for (int i = 0; i < 4; ++i) {
+            dspm::Mat temp(lp[i], 4, 1);
+            dspm::Mat result_vec(4, 1);
+
+            if (i == 1 || i == 3) {
+                result_vec = Ix * ((i == 1 ? Trf.inverse() : Trb.inverse()) * temp);
+            } else {
+                result_vec = (i == 0 ? Tlf.inverse() : Tlb.inverse()) * temp;
+            }
+
+            legIK(result_vec.data, &result[i * 3]);
+        }
+
+        return res;
+    }
+
+    esp_err_t bodyIK(position_t p) {
+        float cos_omega = cos(p.omega*DEGREES2RAD);
+        float sin_omega = sin(p.omega*DEGREES2RAD);
+        float cos_phi = cos(p.phi*DEGREES2RAD);
+        float sin_phi = sin(p.phi*DEGREES2RAD);
+        float cos_psi = cos(p.psi*DEGREES2RAD);
+        float sin_psi = sin(p.psi*DEGREES2RAD);
+
+        float Rx_data[] = {
+            1, 0, 0, 0,
+            0, cos_omega, -sin_omega, 0,
+            0, sin_omega, cos_omega, 0,
+            0, 0, 0, 1
+        };
+        dspm::Mat Rx(Rx_data, 4, 4);
+
+        float Ry_data[] = {
+            cos_phi, 0, sin_phi, 0,
+            0, 1, 0, 0,
+            -sin_phi, 0, cos_phi, 0,
+            0, 0, 0, 1
+        };
+        dspm::Mat Ry(Ry_data, 4, 4);
+
+        float Rz_data[] = {
+            cos_psi, -sin_psi, 0, 0,
+            sin_psi, cos_psi, 0, 0,
+            0, 0, 1, 0,
+            0, 0, 0, 1
+        };
+        dspm::Mat Rz(Rz_data, 4, 4);
+
+        dspm::Mat Rxyz = Rx * Ry * Rz;
+
+        float T_data[] = {
+            0, 0, 0, p.xm,
+            0, 0, 0, p.ym,
+            0, 0, 0, p.zm,
+            0, 0, 0, 0
+        };
+        dspm::Mat T(T_data, 4, 4);
+
+        dspm::Mat Tm = T + Rxyz;
+
+        float sHp = sin(M_PI / 2);
+        float cHp = cos(M_PI / 2);
+
+        float points_lf[] = {
+            cHp, 0, sHp, L / 2,
+            0, 1, 0, 0,
+            -sHp, 0, cHp, W / 2,
+            0, 0, 0, 1
+        };
+        Tlf = Tm * dspm::Mat(points_lf, 4, 4);
+
+        float points_rf[] = {
+            cHp, 0, sHp, L / 2,
+            0, 1, 0, 0,
+            -sHp, 0, cHp, -W / 2,
+            0, 0, 0, 1
+        };
+        Trf = Tm * dspm::Mat(points_rf, 4, 4);
+
+        float points_lb[] = {
+            cHp, 0, sHp, -L / 2,
+            0, 1, 0, 0,
+            -sHp, 0, cHp, W / 2,
+            0, 0, 0, 1
+        };
+        Tlb = Tm * dspm::Mat(points_lb, 4, 4);
+
+        float points_rb[] = {
+            cHp, 0, sHp, -L / 2,
+            0, 1, 0, 0,
+            -sHp, 0, cHp, -W / 2,
+            0, 0, 0, 1
+        };
+        Trb = Tm * dspm::Mat(points_rb, 4, 4);
+        return ESP_OK;
+    }
+
+    void legIK(float point[4], float result[3]) {
+        float x = point[0];
+        float y = point[1];
+        float z = point[2];
+
+        float F = sqrt(x * x + y * y - l1 * l1);
+        if (isnan(F)) F = l1;
+        float G = F - l2;
+        float H = sqrt(G * G + z * z);
+
+        result[0] = -atan2(y, x) - atan2(F, -l1);
+        result[2] = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4));
+        if (isnan(result[2])) result[2] = 0;
+        result[1] = atan2(z, G) - atan2(l4 * sin(result[2]), l3 + l4 * cos(result[2]));
+        result[0] *= RAD2DEGREES;
+        result[1] *= RAD2DEGREES;
+        result[2] *= RAD2DEGREES;
+    }
+};
+
+#endif

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -3,6 +3,7 @@
 
 #include <EventSocket.h>
 #include <TaskManager.h>
+#include <Kinematics.h>
 
 #define DEFAULT_STATE false
 #define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
@@ -96,6 +97,33 @@ class MotionService
                     }
                 }
                 break;
+
+            case MOTION_STATE::STAND: {
+
+                float lp[4][4] = {
+                    { 100, -100,  100, 1},
+                    { 100, -100, -100, 1},
+                    {-100, -100,  100, 1},
+                    {-100, -100, -100, 1}
+                };
+                position_t p = {0, 0, 0, 0, static_cast<float>(input[5]), 0, input[0]};
+                float new_angles[12] = {0,};
+                float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
+
+                kinematics.calculate_inverse_kinematics(lp, p, new_angles);
+
+                for (int i = 0; i < 12; i++) {
+                    int16_t new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
+                    if (new_angle != angles[i]) {
+                        angles[i] = new_angle;
+                        updated = true;
+                    }
+                }
+                if (updated) {
+                    ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %f", new_angles[0], new_angles[1], new_angles[2], new_angles[3], new_angles[4], new_angles[5], new_angles[6], new_angles[7], new_angles[8], new_angles[9], new_angles[10], new_angles[11]);
+                }
+                break;
+            }
             case MOTION_STATE::WALK:
                 angles[1] += dir;
                 if (angles[1] >= 90) dir = -1;
@@ -118,12 +146,14 @@ class MotionService
     EventSocket *_socket;
     SecurityManager *_securityManager;
     TaskManager *_taskManager;
+    Kinematics kinematics;
 
     constexpr static int MotionInterval = 100;
 
     int8_t input[7] = {0, 0, 0, 0, 0, 0, 0};
     int16_t angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
     int16_t rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    int16_t stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
     MOTION_STATE motionState = MOTION_STATE::IDLE;
     unsigned long _lastUpdate;
     int dir = 2;

kinematics.ipynb

@@ -0,0 +1,99 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import sympy as sp\n",
+    "import numpy as np"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 17,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\phi \\right)} \\cos{\\left(\\psi \\right)} & - \\sin{\\left(\\psi \\right)} \\cos{\\left(\\phi \\right)} & \\sin{\\left(\\phi \\right)} & x\\\\\\sin{\\left(\\omega \\right)} \\sin{\\left(\\phi \\right)} \\cos{\\left(\\psi \\right)} + \\sin{\\left(\\psi \\right)} \\cos{\\left(\\omega \\right)} & - \\sin{\\left(\\omega \\right)} \\sin{\\left(\\phi \\right)} \\sin{\\left(\\psi \\right)} + \\cos{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} & - \\sin{\\left(\\omega \\right)} \\cos{\\left(\\phi \\right)} & y\\\\\\sin{\\left(\\omega \\right)} \\sin{\\left(\\psi \\right)} - \\sin{\\left(\\phi \\right)} \\cos{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} & \\sin{\\left(\\omega \\right)} \\cos{\\left(\\psi \\right)} + \\sin{\\left(\\phi \\right)} \\sin{\\left(\\psi \\right)} \\cos{\\left(\\omega \\right)} & \\cos{\\left(\\omega \\right)} \\cos{\\left(\\phi \\right)} & z\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
+      ],
+      "text/plain": [
+       "Matrix([\n",
+       "[                                 cos(phi)*cos(psi),                                  -sin(psi)*cos(phi),             sin(phi), x],\n",
+       "[sin(omega)*sin(phi)*cos(psi) + sin(psi)*cos(omega), -sin(omega)*sin(phi)*sin(psi) + cos(omega)*cos(psi), -sin(omega)*cos(phi), y],\n",
+       "[sin(omega)*sin(psi) - sin(phi)*cos(omega)*cos(psi),  sin(omega)*cos(psi) + sin(phi)*sin(psi)*cos(omega),  cos(omega)*cos(phi), z],\n",
+       "[                                                 0,                                                   0,                    0, 1]])"
+      ]
+     },
+     "execution_count": 17,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "omega = sp.symbols('omega')\n",
+    "psi = sp.symbols('psi')\n",
+    "phi = sp.symbols('phi')\n",
+    "x = sp.symbols('x')\n",
+    "y = sp.symbols('y')\n",
+    "z = sp.symbols('z')\n",
+    "\n",
+    "\n",
+    "rx = sp.Matrix([\n",
+    "    [1, 0, 0, 0],\n",
+    "    [0, sp.cos(omega), -sp.sin(omega), 0],\n",
+    "    [0, sp.sin(omega), sp.cos(omega), 0],\n",
+    "    [0, 0, 0, 1],\n",
+    "])\n",
+    "\n",
+    "ry = sp.Matrix([\n",
+    "    [sp.cos(phi), 0, sp.sin(phi), 0, ],\n",
+    "    [0, 1, 0, 0],\n",
+    "    [-sp.sin(phi), 0, sp.cos(phi), 0],\n",
+    "    [0, 0, 0, 1],\n",
+    "])\n",
+    "\n",
+    "rz = sp.Matrix([\n",
+    "    [sp.cos(psi), -sp.sin(psi), 0, 0],\n",
+    "    [sp.sin(psi), sp.cos(psi), 0, 0],\n",
+    "    [0, 0, 1, 0],\n",
+    "    [0, 0, 0, 1],\n",
+    "])\n",
+    "\n",
+    "t = sp.Matrix([\n",
+    "    [0, 0, 0, x],\n",
+    "    [0, 0, 0, y],\n",
+    "    [0, 0, 0, z],\n",
+    "    [0, 0, 0, 0],\n",
+    "])\n",
+    "\n",
+    "tm = t + rx * ry * rz\n",
+    "tm"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.11.5"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}