⚡️Improve eventbus static alloc

https://github.com/Jerome-Graves/yertle/

.github/workflows/embedded-build.yml

@@ -2,20 +2,19 @@ name: PlatformIO CI
 
 on:
   push:
-    branches: [ master ]
+    branches: [master]
     paths:
-        - 'esp32/**'  
+      - "esp32/**"
+      - "platformio.ini"
   pull_request:
-    branches: [ master ]
+    branches: [master]
     paths:
-        - 'esp32/**'  
+      - "esp32/**"
+      - "platformio.ini"
 
 jobs:
   build:
     runs-on: ubuntu-latest
-    defaults:
-      run:
-        working-directory: ./esp32
 
     steps:
       - uses: actions/checkout@v3
@@ -28,8 +27,8 @@ jobs:
 
       - uses: actions/setup-python@v4
         with:
-          python-version: '3.x'
-      - run: pip install -r ./scripts/requirements.txt
+          python-version: "3.x"
+      - run: pip install -r esp32/scripts/requirements.txt
       - name: Install PlatformIO Core
         run: pip install --upgrade platformio
 

.gitignore

@@ -5,4 +5,5 @@
 
 __pycache__/
 *.py[cod]
-*$py.class
+*$py.class
+.pio

app/README.md

@@ -8,30 +8,21 @@ If you're seeing this, you've probably already done this step. Congrats!
 
 ```bash
 # create a new project in the current directory
-npm create svelte@latest
-
-# create a new project in my-app
-npm create svelte@latest my-app
+npx sv create
 ```
 
 ## Developing
 
-Once you've created a project and installed dependencies with `npm install` (or `pnpm install` or `yarn`), start a development server:
+Once you've created your project, follow these steps:
 
-```bash
-npm run dev
+1: Delete package-lock.json
+2: Check `git status`. If you see any changes other than package-lock.json or favicon.ico, run the command `git restore ./` (See below)
+3: Run `npm install` or `pnpm install` or `yarn` to install the dependencies
+4: Run `npm run build` to build the project
 
-# or start the server and open the app in a new browser tab
-npm run dev -- --open
-```
+Running `git status` should show:
 
-## Building
-
-To create a production version of your app:
-
-```bash
-npm run build
-```
+[![example.png](https://i.postimg.cc/yddM3hH3/example.png)](https://postimg.cc/7CFsp2bq)
 
 You can preview the production build with `npm run preview`.
 

app/package.json

@@ -59,7 +59,9 @@
     "three": "^0.162.0",
     "urdf-loader": "^0.12.1",
     "uzip": "^0.20201231.0",
-    "xacro-parser": "^0.3.9"
+    "xacro-parser": "^0.3.9",
+    "@types/msgpack-lite": "^0.1.11",
+    "msgpack-lite": "^0.1.26"
   },
   "packageManager": "pnpm@9.3.0"
 }

app/pnpm-lock.yaml

@@ -13,10 +13,13 @@ importers:
         version: 1.1.2
       '@sveltejs/adapter-auto':
         specifier: ^4.0.0
-        version: 4.0.0(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))
+        version: 4.0.0(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))
       '@tailwindcss/vite':
         specifier: ^4.0.12
-        version: 4.0.12(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+        version: 4.0.12(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@types/msgpack-lite':
+        specifier: ^0.1.11
+        version: 0.1.11
       chart.js:
         specifier: ^4.4.2
         version: 4.4.2
@@ -32,6 +35,9 @@ importers:
       jwt-decode:
         specifier: ^4.0.0
         version: 4.0.0
+      msgpack-lite:
+        specifier: ^0.1.26
+        version: 0.1.26
       nipplejs:
         specifier: ^0.10.1
         version: 0.10.1
@@ -65,13 +71,13 @@ importers:
         version: 1.49.1
       '@sveltejs/adapter-static':
         specifier: ^3.0.1
-        version: 3.0.1(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))
+        version: 3.0.1(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))
       '@sveltejs/kit':
         specifier: ^2.5.27
-        version: 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+        version: 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       '@sveltejs/vite-plugin-svelte':
         specifier: ^5.0.3
-        version: 5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+        version: 5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       '@types/eslint':
         specifier: ^8.56.0
         version: 8.56.0
@@ -128,10 +134,10 @@ importers:
         version: 0.18.5
       vite:
         specifier: ^6.2.1
-        version: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+        version: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
       vitest:
         specifier: ^1.2.0
-        version: 1.2.0(jsdom@24.0.0)(lightningcss@1.29.2)
+        version: 1.2.0(@types/node@24.0.10)(jsdom@24.0.0)(lightningcss@1.29.2)
 
 packages:
 
@@ -760,6 +766,12 @@ packages:
   '@types/json-schema@7.0.15':
     resolution: {integrity: sha512-5+fP8P8MFNC+AyZCDxrB2pkZFPGzqQWUzpSeuuVLvm8VMcorNYavBqoFcxK8bQz4Qsbn4oUEEem4wDLfcysGHA==}
 
+  '@types/msgpack-lite@0.1.11':
+    resolution: {integrity: sha512-cdCZS/gw+jIN22I4SUZUFf1ZZfVv5JM1//Br/MuZcI373sxiy3eSSoiyLu0oz+BPatTbGGGBO5jrcvd0siCdTQ==}
+
+  '@types/node@24.0.10':
+    resolution: {integrity: sha512-ENHwaH+JIRTDIEEbDK6QSQntAYGtbvdDXnMXnZaZ6k13Du1dPMmprkEHIL7ok2Wl2aZevetwTAb5S+7yIF+enA==}
+
   '@types/semver@7.5.8':
     resolution: {integrity: sha512-I8EUhyrgfLrcTkzV3TSsGyl1tSuPrEDzr0yd5m90UgNxQkyDXULk3b6MlQqTCpZpNtWe1K0hzclnZkTcLBe2UQ==}
 
@@ -1190,6 +1202,9 @@ packages:
     resolution: {integrity: sha512-kVscqXk4OCp68SZ0dkgEKVi6/8ij300KBWTJq32P/dYeWTSwK41WyTxalN1eRmA5Z9UU/LX9D7FWSmV9SAYx6g==}
     engines: {node: '>=0.10.0'}
 
+  event-lite@0.1.3:
+    resolution: {integrity: sha512-8qz9nOz5VeD2z96elrEKD2U433+L3DWdUdDkOINLGOJvx1GsMBbMn0aCeu28y8/e85A6mCigBiFlYMnTBEGlSw==}
+
   execa@5.1.1:
     resolution: {integrity: sha512-8uSpZZocAZRBAPIEINJj3Lo9HyGitllczc27Eh5YYojjMFMn8yHMDMaUHE2Jqfq05D/wucwI4JGURyXt1vchyg==}
     engines: {node: '>=10'}
@@ -1335,6 +1350,9 @@ packages:
     resolution: {integrity: sha512-4fCk79wshMdzMp2rH06qWrJE4iolqLhCUH+OiuIgU++RB0+94NlDL81atO7GX55uUKueo0txHNtvEyI6D7WdMw==}
     engines: {node: '>=0.10.0'}
 
+  ieee754@1.2.1:
+    resolution: {integrity: sha512-dcyqhDvX1C46lXZcVqCpK+FtMRQVdIMN6/Df5js2zouUsqG7I6sFxitIC+7KYK29KdXOLHdu9zL4sFnoVQnqaA==}
+
   ignore@5.3.1:
     resolution: {integrity: sha512-5Fytz/IraMjqpwfd34ke28PTVMjZjJG2MPn5t7OE4eUCUNf8BAa7b5WUS9/Qvr6mwOQS7Mk6vdsMno5he+T8Xw==}
     engines: {node: '>= 4'}
@@ -1356,6 +1374,9 @@ packages:
   inherits@2.0.4:
     resolution: {integrity: sha512-k/vGaX4/Yla3WzyMCvTQOXYeIHvqOKtnqBduzTHpzpQZzAskKMhZ2K+EnBiSM9zGSoIFeMpXKxa4dYeZIQqewQ==}
 
+  int64-buffer@0.1.10:
+    resolution: {integrity: sha512-v7cSY1J8ydZ0GyjUHqF+1bshJ6cnEVLo9EnjB8p+4HDRPZc9N5jjmvUV7NvEsqQOKyH0pmIBFWXVQbiS0+OBbA==}
+
   is-binary-path@2.1.0:
     resolution: {integrity: sha512-ZMERYes6pDydyuGidse7OsHxtbI7WVeUEozgR/g7rd0xUimYNlvZRE/K2MgZTjWy725IfelLeVcEM97mmtRGXw==}
     engines: {node: '>=8'}
@@ -1394,6 +1415,9 @@ packages:
     resolution: {integrity: sha512-LnQR4bZ9IADDRSkvpqMGvt/tEJWclzklNgSw48V5EAaAeDd6qGvN8ei6k5p0tvxSR171VmGyHuTiAOfxAbr8kA==}
     engines: {node: ^12.20.0 || ^14.13.1 || >=16.0.0}
 
+  isarray@1.0.0:
+    resolution: {integrity: sha512-VLghIWNM6ELQzo7zwmcg0NmTVyWKYjvIeM83yjp0wRDTmUnrM678fQbcKBo6n2CJEF0szoG//ytg+TKla89ALQ==}
+
   isexe@2.0.0:
     resolution: {integrity: sha512-RHxMLp9lnKHGHRng9QFhRCMbYAcVpn69smSGcq3f36xjgVVWThj4qqLbTLlq7Ssj8B+fIQ1EuCEGI2lKsyQeIw==}
 
@@ -1592,6 +1616,10 @@ packages:
   ms@2.1.3:
     resolution: {integrity: sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==}
 
+  msgpack-lite@0.1.26:
+    resolution: {integrity: sha512-SZ2IxeqZ1oRFGo0xFGbvBJWMp3yLIY9rlIJyxy8CGrwZn1f0ZK4r6jV/AM1r0FZMDUkWkglOk/eeKIL9g77Nxw==}
+    hasBin: true
+
   nanoid@3.3.7:
     resolution: {integrity: sha512-eSRppjcPIatRIMC1U6UngP8XFcz8MQWGQdt1MTBQ7NaAmvXDfvNxbvWV3x2y6CdEUciCSsDHDQZbhYaB8QEo2g==}
     engines: {node: ^10 || ^12 || ^13.7 || ^14 || >=15.0.1}
@@ -2019,6 +2047,9 @@ packages:
   ufo@1.5.3:
     resolution: {integrity: sha512-Y7HYmWaFwPUmkoQCUIAYpKqkOf+SbVj/2fJJZ4RJMCfZp0rTGwRbzQD+HghfnhKOjL9E01okqz+ncJskGYfBNw==}
 
+  undici-types@7.8.0:
+    resolution: {integrity: sha512-9UJ2xGDvQ43tYyVMpuHlsgApydB8ZKfVYTsLDhXkFL/6gfkp+U8xTGdh8pMJv1SpZna0zxG1DwsKZsreLbXBxw==}
+
   universalify@0.2.0:
     resolution: {integrity: sha512-CJ1QgKmNg3CwvAv/kOFmtnEN05f0D/cn9QntgNOQlQF9dgvVTHj3t+8JPdjqawCHk7V/KA+fbUqzZ9XWhcqPUg==}
     engines: {node: '>= 4.0.0'}
@@ -2613,18 +2644,18 @@ snapshots:
 
   '@sinclair/typebox@0.27.8': {}
 
-  '@sveltejs/adapter-auto@4.0.0(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))':
+  '@sveltejs/adapter-auto@4.0.0(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))':
     dependencies:
-      '@sveltejs/kit': 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@sveltejs/kit': 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       import-meta-resolve: 4.1.0
 
-  '@sveltejs/adapter-static@3.0.1(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))':
+  '@sveltejs/adapter-static@3.0.1(@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))':
     dependencies:
-      '@sveltejs/kit': 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@sveltejs/kit': 2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
 
-  '@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
+  '@sveltejs/kit@2.17.3(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
     dependencies:
-      '@sveltejs/vite-plugin-svelte': 5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@sveltejs/vite-plugin-svelte': 5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       '@types/cookie': 0.6.0
       cookie: 0.6.0
       devalue: 5.1.1
@@ -2637,27 +2668,27 @@ snapshots:
       set-cookie-parser: 2.6.0
       sirv: 3.0.1
       svelte: 5.20.4
-      vite: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+      vite: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
 
-  '@sveltejs/vite-plugin-svelte-inspector@4.0.1(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
+  '@sveltejs/vite-plugin-svelte-inspector@4.0.1(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
     dependencies:
-      '@sveltejs/vite-plugin-svelte': 5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@sveltejs/vite-plugin-svelte': 5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       debug: 4.4.0
       svelte: 5.20.4
-      vite: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+      vite: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
     transitivePeerDependencies:
       - supports-color
 
-  '@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
+  '@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
     dependencies:
-      '@sveltejs/vite-plugin-svelte-inspector': 4.0.1(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      '@sveltejs/vite-plugin-svelte-inspector': 4.0.1(@sveltejs/vite-plugin-svelte@5.0.3(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)))(svelte@5.20.4)(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
       debug: 4.4.0
       deepmerge: 4.3.1
       kleur: 4.1.5
       magic-string: 0.30.17
       svelte: 5.20.4
-      vite: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
-      vitefu: 1.0.6(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
+      vite: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+      vitefu: 1.0.6(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))
     transitivePeerDependencies:
       - supports-color
 
@@ -2714,13 +2745,13 @@ snapshots:
       '@tailwindcss/oxide-win32-arm64-msvc': 4.0.12
       '@tailwindcss/oxide-win32-x64-msvc': 4.0.12
 
-  '@tailwindcss/vite@4.0.12(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
+  '@tailwindcss/vite@4.0.12(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2))':
     dependencies:
       '@tailwindcss/node': 4.0.12
       '@tailwindcss/oxide': 4.0.12
       lightningcss: 1.29.2
       tailwindcss: 4.0.12
-      vite: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+      vite: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
 
   '@tweenjs/tween.js@23.1.2': {}
 
@@ -2737,6 +2768,14 @@ snapshots:
 
   '@types/json-schema@7.0.15': {}
 
+  '@types/msgpack-lite@0.1.11':
+    dependencies:
+      '@types/node': 24.0.10
+
+  '@types/node@24.0.10':
+    dependencies:
+      undici-types: 7.8.0
+
   '@types/semver@7.5.8': {}
 
   '@types/stats.js@0.17.3': {}
@@ -3259,6 +3298,8 @@ snapshots:
 
   esutils@2.0.3: {}
 
+  event-lite@0.1.3: {}
+
   execa@5.1.1:
     dependencies:
       cross-spawn: 7.0.3
@@ -3414,6 +3455,8 @@ snapshots:
     dependencies:
       safer-buffer: 2.1.2
 
+  ieee754@1.2.1: {}
+
   ignore@5.3.1: {}
 
   import-fresh@3.3.0:
@@ -3432,6 +3475,8 @@ snapshots:
 
   inherits@2.0.4: {}
 
+  int64-buffer@0.1.10: {}
+
   is-binary-path@2.1.0:
     dependencies:
       binary-extensions: 2.3.0
@@ -3458,6 +3503,8 @@ snapshots:
 
   is-stream@3.0.0: {}
 
+  isarray@1.0.0: {}
+
   isexe@2.0.0: {}
 
   jiti@2.4.2: {}
@@ -3635,6 +3682,13 @@ snapshots:
 
   ms@2.1.3: {}
 
+  msgpack-lite@0.1.26:
+    dependencies:
+      event-lite: 0.1.3
+      ieee754: 1.2.1
+      int64-buffer: 0.1.10
+      isarray: 1.0.0
+
   nanoid@3.3.7: {}
 
   nanoid@3.3.8: {}
@@ -4010,6 +4064,8 @@ snapshots:
 
   ufo@1.5.3: {}
 
+  undici-types@7.8.0: {}
+
   universalify@0.2.0: {}
 
   unplugin-icons@0.18.5:
@@ -4054,13 +4110,13 @@ snapshots:
 
   uzip@0.20201231.0: {}
 
-  vite-node@1.2.0(lightningcss@1.29.2):
+  vite-node@1.2.0(@types/node@24.0.10)(lightningcss@1.29.2):
     dependencies:
       cac: 6.7.14
       debug: 4.4.0
       pathe: 1.1.2
       picocolors: 1.0.1
-      vite: 5.4.14(lightningcss@1.29.2)
+      vite: 5.4.14(@types/node@24.0.10)(lightningcss@1.29.2)
     transitivePeerDependencies:
       - '@types/node'
       - less
@@ -4072,31 +4128,33 @@ snapshots:
       - supports-color
       - terser
 
-  vite@5.4.14(lightningcss@1.29.2):
+  vite@5.4.14(@types/node@24.0.10)(lightningcss@1.29.2):
     dependencies:
       esbuild: 0.21.5
       postcss: 8.5.3
       rollup: 4.34.8
     optionalDependencies:
+      '@types/node': 24.0.10
       fsevents: 2.3.3
       lightningcss: 1.29.2
 
-  vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2):
+  vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2):
     dependencies:
       esbuild: 0.25.0
       postcss: 8.5.3
       rollup: 4.34.8
     optionalDependencies:
+      '@types/node': 24.0.10
       fsevents: 2.3.3
       jiti: 2.4.2
       lightningcss: 1.29.2
       yaml: 2.4.2
 
-  vitefu@1.0.6(vite@6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)):
+  vitefu@1.0.6(vite@6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)):
     optionalDependencies:
-      vite: 6.2.1(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
+      vite: 6.2.1(@types/node@24.0.10)(jiti@2.4.2)(lightningcss@1.29.2)(yaml@2.4.2)
 
-  vitest@1.2.0(jsdom@24.0.0)(lightningcss@1.29.2):
+  vitest@1.2.0(@types/node@24.0.10)(jsdom@24.0.0)(lightningcss@1.29.2):
     dependencies:
       '@vitest/expect': 1.2.0
       '@vitest/runner': 1.2.0
@@ -4116,10 +4174,11 @@ snapshots:
       strip-literal: 1.3.0
       tinybench: 2.8.0
       tinypool: 0.8.4
-      vite: 5.4.14(lightningcss@1.29.2)
-      vite-node: 1.2.0(lightningcss@1.29.2)
+      vite: 5.4.14(@types/node@24.0.10)(lightningcss@1.29.2)
+      vite-node: 1.2.0(@types/node@24.0.10)(lightningcss@1.29.2)
       why-is-node-running: 2.2.2
     optionalDependencies:
+      '@types/node': 24.0.10
       jsdom: 24.0.0
     transitivePeerDependencies:
       - less

app/src/app.css

@@ -2,39 +2,39 @@
 @plugin "daisyui";
 
 @plugin "daisyui" {
-  themes:
-    light --default,
-    dark --prefersdark;
+    themes:
+        light --default,
+        dark --prefersdark;
 }
 
 @plugin "daisyui/theme" {
-  name: 'light';
-  default: true;
-  --color-primary: #00bfff;
-  --color-secondary: #3c00ff;
-  --base-content: white;
+    name: 'light';
+    default: true;
+    --color-primary: #00bfff;
+    --color-secondary: #3c00ff;
+    --base-content: white;
 }
 
 @plugin "daisyui/theme" {
-  name: 'dark';
-  prefersdark: true;
-  --color-primary: #00bfff;
-  --color-secondary: #3c00ff;
-  --base-content: oklch(0.3 0.012 256);
+    name: 'dark';
+    prefersdark: true;
+    --color-primary: #00bfff;
+    --color-secondary: #3c00ff;
+    --base-content: oklch(0.3 0.012 256);
 }
 
 #nipple_0_0,
 #nipple_1_1 {
-  z-index: 10 !important;
+    z-index: 10 !important;
 }
 
 #three-gui-panel {
-  top: 64px;
-  right: 0px;
+    top: 64px;
+    right: 0px;
 }
 
 @media (max-width: 1023px) {
-  #three-gui-panel {
-    top: 48px;
-  }
+    #three-gui-panel {
+        top: 48px;
+    }
 }

app/src/lib/components/ConfirmDialog.svelte

@@ -1,61 +1,43 @@
 <script lang="ts">
-    import { focusTrap } from 'svelte-focus-trap';
-    import { fly } from 'svelte/transition';
-    import { Cancel, Check } from '$lib/components/icons';
-    import { modals, exitBeforeEnter } from 'svelte-modals';
+  import { focusTrap } from 'svelte-focus-trap'
+  import { fly } from 'svelte/transition'
+  import { Cancel, Check } from '$lib/components/icons'
+  import { modals, exitBeforeEnter, type ModalProps } from 'svelte-modals'
 
-    // provided by <Modals />
-
-    interface Props {
-        isOpen: boolean;
-        title: string;
-        message: string;
-        onConfirm: any;
-        labels?: any;
+  let {
+    isOpen,
+    title,
+    message,
+    onConfirm,
+    labels = {
+      cancel: { label: 'Cancel', icon: Cancel },
+      confirm: { label: 'OK', icon: Check }
     }
-
-    let {
-        isOpen,
-        title,
-        message,
-        onConfirm,
-        labels = {
-            cancel: { label: 'Cancel', icon: Cancel },
-            confirm: { label: 'OK', icon: Check }
-        }
-    }: Props = $props();
+  }: ModalProps = $props()
 </script>
 
 {#if isOpen}
-    {@const SvelteComponent = labels?.confirm.icon}
+  {@const SvelteComponent = labels?.confirm.icon}
+  <div
+    role="dialog"
+    class="pointer-events-none fixed inset-0 z-50 flex items-center justify-center"
+    transition:fly={{ y: 50 }}
+    use:exitBeforeEnter
+    use:focusTrap>
     <div
-        role="dialog"
-        class="pointer-events-none fixed inset-0 z-50 flex items-center justify-center"
-        transition:fly={{ y: 50 }}
-        use:exitBeforeEnter
-        use:focusTrap
-    >
-        <div
-            class="rounded-box bg-base-100 pointer-events-auto flex min-w-fit max-w-md flex-col justify-between p-4 shadow-lg"
-        >
-            <h2 class="text-base-content text-start text-2xl font-bold">{title}</h2>
-            <div class="divider my-2"></div>
-            <p class="text-base-content mb-1 text-start">{message}</p>
-            <div class="divider my-2"></div>
-            <div class="flex justify-end gap-2">
-                <button
-                    class="btn btn-primary inline-flex items-center"
-                    onclick={() => modals.close()}
-                >
-                    <labels.cancel.icon class="mr-2 h-5 w-5" /><span>{labels?.cancel.label}</span>
-                </button>
-                <button
-                    class="btn btn-warning text-warning-content inline-flex items-center"
-                    onclick={onConfirm}
-                >
-                    <SvelteComponent class="mr-2 h-5 w-5" /><span>{labels?.confirm.label}</span>
-                </button>
-            </div>
-        </div>
+      class="rounded-box bg-base-100 pointer-events-auto flex min-w-fit max-w-md flex-col justify-between p-4 shadow-lg">
+      <h2 class="text-base-content text-start text-2xl font-bold">{title}</h2>
+      <div class="divider my-2"></div>
+      <p class="text-base-content mb-1 text-start">{message}</p>
+      <div class="divider my-2"></div>
+      <div class="flex justify-end gap-2">
+        <button class="btn btn-error inline-flex items-center" onclick={() => modals.close()}>
+          <labels.cancel.icon class="mr-2 h-5 w-5" /><span>{labels?.cancel.label}</span>
+        </button>
+        <button class="btn btn-primary inline-flex items-center" onclick={onConfirm}>
+          <SvelteComponent class="mr-2 h-5 w-5" /><span>{labels?.confirm.label}</span>
+        </button>
+      </div>
     </div>
+  </div>
 {/if}

app/src/lib/components/OrientationIndicator.svelte

@@ -0,0 +1,78 @@
+<script lang="ts">
+  import { onMount, onDestroy } from 'svelte'
+  import * as THREE from 'three'
+  import { imu } from '$lib/stores/imu'
+  import SceneBuilder from '$lib/sceneBuilder'
+
+  let canvas: HTMLCanvasElement = $state()
+  let sceneBuilder: SceneBuilder
+  let cube: THREE.Mesh
+  let targetRotation = new THREE.Euler()
+  let lastUpdateTime = 0
+  const LERP_SPEED = 5 // rotations per second
+
+  const initThreeJS = () => {
+    sceneBuilder = new SceneBuilder()
+      .addRenderer({ canvas: canvas, antialias: true, alpha: true })
+      .addPerspectiveCamera({ x: 2, y: 0, z: 2 })
+      .addOrbitControls(1, 10, false)
+      .addAmbientLight({ color: 0x404040, intensity: 0.5 })
+      .addDirectionalLight({ color: 0xffffff, intensity: 3, x: 10, y: 20, z: 7 })
+      .fillParent()
+
+    const geometry = new THREE.BoxGeometry(1, 1, 1)
+    const material = new THREE.MeshPhongMaterial({
+      color: 0x00ff00,
+      transparent: true,
+      opacity: 0.8
+    })
+    cube = new THREE.Mesh(geometry, material)
+    sceneBuilder.scene.add(cube)
+
+    sceneBuilder.addRenderCb(() => {
+      if (!cube) return
+      const currentTime = performance.now()
+      const deltaTime = (currentTime - lastUpdateTime) / 1000 // convert to seconds
+      lastUpdateTime = currentTime
+
+      const lerpFactor = Math.min(1, LERP_SPEED * deltaTime)
+      cube.rotation.x = THREE.MathUtils.lerp(cube.rotation.x, targetRotation.x, lerpFactor)
+      cube.rotation.y = THREE.MathUtils.lerp(cube.rotation.y, targetRotation.y, lerpFactor)
+      cube.rotation.z = THREE.MathUtils.lerp(cube.rotation.z, targetRotation.z, lerpFactor)
+    })
+
+    sceneBuilder.startRenderLoop()
+  }
+
+  const updateOrientation = () => {
+    if (!cube) return
+
+    const y = -$imu.x[$imu.x.length - 1] || 0
+    const x = $imu.y[$imu.y.length - 1] || 0
+    const z = -$imu.z[$imu.z.length - 1] || 0
+
+    targetRotation.set(
+      THREE.MathUtils.degToRad(x),
+      THREE.MathUtils.degToRad(y),
+      THREE.MathUtils.degToRad(z)
+    )
+  }
+
+  onMount(() => {
+    initThreeJS()
+  })
+
+  onDestroy(() => {
+    sceneBuilder?.renderer?.dispose()
+  })
+
+  $effect(() => {
+    if ($imu) {
+      updateOrientation()
+    }
+  })
+</script>
+
+<div class="h-60 w-60 border-2 border-base-300 rounded-md">
+  <canvas class="w-full h-full" bind:this={canvas}></canvas>
+</div>

app/src/lib/components/SettingsCard.svelte

@@ -1,69 +1,60 @@
 <script lang="ts">
-	import { slide } from 'svelte/transition';
-	import { cubicOut } from 'svelte/easing';
-	import { Down } from './icons';
-	interface Props {
-		open?: boolean;
-		collapsible?: boolean;
-		icon?: import('svelte').Snippet;
-		title?: import('svelte').Snippet;
-		children?: import('svelte').Snippet;
-	}
+  import { slide } from 'svelte/transition'
+  import { cubicOut } from 'svelte/easing'
+  import { Down } from './icons'
+  interface Props {
+    open?: boolean
+    collapsible?: boolean
+    icon?: import('svelte').Snippet
+    title?: import('svelte').Snippet
+    children?: import('svelte').Snippet
+    right?: import('svelte').Snippet
+  }
 
-	let {
-		open = $bindable(true),
-		collapsible = true,
-		icon,
-		title,
-		children
-	}: Props = $props();
+  let { open = $bindable(true), collapsible = true, icon, title, children, right }: Props = $props()
 </script>
 
 {#if collapsible}
-	<div
-		class="bg-base-200 rounded-box relative grid w-full max-w-2xl self-center overflow-hidden shadow-lg"
-	>
-		<div
-			class="min-h-16 flex w-full items-center justify-between space-x-3 p-4 text-xl font-medium"
-		>
-			<span class="inline-flex items-baseline">
-				{@render icon?.()}
-				{@render title?.()}
-			</span>
-			<button
-				class="btn btn-circle btn-ghost btn-sm"
-				onclick={() => {
-					open = !open;
-				}}
-			>
-				<Down
-					class="text-base-content h-auto w-6 transition-transform duration-300 ease-in-out {open
-						? 'rotate-180'
-						: ''}"
-				/>
-			</button>
-		</div>
-		{#if open}
-			<div
-				class="flex flex-col gap-2 p-4 pt-0"
-				transition:slide|local={{ duration: 300, easing: cubicOut }}
-			>
-				{@render children?.()}
-			</div>
-		{/if}
-	</div>
+  <div
+    class="bg-base-200 rounded-box relative grid w-full max-w-2xl self-center overflow-hidden shadow-lg">
+    <div
+      class="min-h-16 flex w-full items-center justify-between space-x-3 p-4 text-xl font-medium">
+      <span class="inline-flex items-baseline">
+        {@render icon?.()}
+        {@render title?.()}
+      </span>
+      <button
+        class="btn btn-circle btn-ghost btn-sm"
+        onclick={() => {
+          open = !open
+        }}>
+        <Down
+          class="text-base-content h-auto w-6 transition-transform duration-300 ease-in-out {open ?
+            'rotate-180'
+          : ''}" />
+      </button>
+    </div>
+    {#if open}
+      <div
+        class="flex flex-col gap-2 p-4 pt-0"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        {@render children?.()}
+      </div>
+    {/if}
+  </div>
 {:else}
-	<div
-		class="bg-base-200 rounded-box relative grid w-full max-w-2xl self-center overflow-hidden shadow-lg"
-	>
-		<div class="min-h-16 w-full p-4 text-xl font-medium">
-			<span class="inline-flex items-baseline">
-				{@render icon?.()}
-				{@render title?.()}
-			</span>
-		</div>
-		<div class="flex flex-col gap-2 p-4 pt-0">
-			{@render children?.()}
-		</div>
-	</div>
+  <div
+    class="bg-base-200 rounded-box relative grid w-full max-w-2xl self-center overflow-hidden shadow-lg">
+    <div
+      class="min-h-16 flex w-full items-center justify-between space-x-3 p-4 text-xl font-medium">
+      <span class="inline-flex items-baseline">
+        {@render icon?.()}
+        {@render title?.()}
+      </span>
+      {@render right?.()}
+    </div>
+    <div class="flex flex-col gap-2 p-4 pt-0">
+      {@render children?.()}
+    </div>
+  </div>
 {/if}

app/src/lib/components/StatusItem.svelte

@@ -0,0 +1,45 @@
+<script lang="ts">
+  type Variant = 'success' | 'error' | 'primary' | 'info' | 'warning'
+
+  const {
+    icon,
+    title,
+    description = '',
+    variant = 'primary',
+    class: klass = '',
+    children = null
+  } = $props<{
+    icon?: any
+    title: string
+    description?: string | number
+    variant?: Variant
+    class?: string
+    children?: () => any
+  }>()
+
+  const Icon = $derived(icon)
+
+  const variants: Record<Variant, [string, string]> = {
+    success: ['bg-success', 'text-success-content'],
+    error: ['bg-error', 'text-error-content'],
+    primary: ['bg-primary', 'text-primary-content'],
+    info: ['bg-info', 'text-info-content'],
+    warning: ['bg-warning', 'text-warning-content']
+  }
+
+  const variantKey: Variant = (variant as Variant) in variants ? (variant as Variant) : 'primary'
+  const [bgColor, textColor] = variants[variantKey]
+</script>
+
+<div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2 {klass}">
+  {#if icon}
+    <div class="mask mask-hexagon {bgColor} h-auto w-10 flex-none">
+      <Icon class="{textColor} h-auto w-full scale-75" />
+    </div>
+  {/if}
+  <div class="grow">
+    <div class="font-bold">{title}</div>
+    <div class="text-sm opacity-75 grow">{description}</div>
+  </div>
+  {@render children?.()}
+</div>

app/src/lib/components/Visualization.svelte

@@ -23,7 +23,12 @@
     mpu,
     jointNames
   } from '$lib/stores'
-  import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities'
+  import {
+    extractFootColor,
+    populateModelCache,
+    throttler,
+    getToeWorldPositions
+  } from '$lib/utilities'
   import SceneBuilder from '$lib/sceneBuilder'
   import { lerp, degToRad } from 'three/src/math/MathUtils'
   import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
@@ -47,17 +52,9 @@
     panel?: boolean
     debug?: boolean
     ground?: boolean
-    zoom?: number
   }
 
-  let {
-    sky = true,
-    orbit = false,
-    panel = true,
-    debug = false,
-    ground = true,
-    zoom = 8
-  }: Props = $props()
+  let { sky = true, orbit = false, panel = true, debug = false, ground = true }: Props = $props()
 
   let sceneManager = $state(new SceneBuilder())
   let canvas: HTMLCanvasElement = $state()
@@ -95,7 +92,7 @@
     xm: 0,
     ym: 0.5,
     zm: 0,
-    feet: planners[ModesEnum.Idle].default_feet_pos
+    feet: kinematic.getDefaultFeetPos()
   }
 
   let settings = {
@@ -180,7 +177,7 @@
     sceneManager
       .addRenderer({ antialias: true, canvas, alpha: true })
       .addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
-      .addOrbitControls(Math.min(zoom, 8), 30, orbit)
+      .addOrbitControls(8, 30, orbit)
       .addDirectionalLight({ x: 10, y: 20, z: 10, color: 0xffffff, intensity: 3 })
       .addAmbientLight({ color: 0xffffff, intensity: 0.5 })
       .addFogExp2(0xcccccc, 0.015)
@@ -204,7 +201,7 @@
 
     for (let i = 0; i < 4; i++) {
       const geometry = new BufferGeometry()
-      const material = new LineBasicMaterial({ color: footColor() })
+      const material = new LineBasicMaterial({ color: extractFootColor() })
       const line = new Line(geometry, material)
       trace_lines.push(geometry)
       sceneManager.scene.add(line)
@@ -276,7 +273,7 @@
       s: controlData[6],
       s1: controlData[7]
     }
-    body_state.ym = ((data.h + 127) * 0.75) / 100
+    body_state.ym = ((data.h + 127) * 0.35) / 100
 
     let planner = planners[get(mode)]
     const delta = performance.now() - lastTick
@@ -311,7 +308,7 @@
     const robot = sceneManager.model
     if (!robot) return
 
-    const toes = toeWorldPositions(robot)
+    const toes = getToeWorldPositions(robot)
 
     renderTraceLines(toes)
     update_camera(robot)

app/src/lib/components/icons/index.ts

@@ -35,6 +35,9 @@ export { default as Hamburger } from '~icons/mdi/hamburger-menu'
 export { default as FileIcon } from '~icons/mdi/file'
 export { default as FolderIcon } from '~icons/mdi/folder-outline'
 export { default as FolderOpenOutline } from '~icons/mdi/folder-open-outline'
+export { default as TrashIcon } from '~icons/mdi/trash'
+export { default as RotateCcw } from '~icons/mdi/rotate-left'
+export { default as RotateCw } from '~icons/mdi/rotate-right'
 
 export { default as Down } from '~icons/tabler/chevron-down'
 export { default as Cancel } from '~icons/tabler/x'
@@ -50,13 +53,14 @@ export { default as Power } from '~icons/tabler/power'
 export { default as MAC } from '~icons/tabler/dna-2'
 export { default as Home } from '~icons/tabler/home'
 export { default as SSID } from '~icons/tabler/router'
-export { default as DNS } from '~icons/tabler/address-book'
+export { default as DNS } from '~icons/mdi/dns'
 export { default as Gateway } from '~icons/tabler/torii'
 export { default as Subnet } from '~icons/tabler/grid-dots'
 export { default as Channel } from '~icons/tabler/antenna'
 export { default as Scan } from '~icons/tabler/radar-2'
 export { default as Add } from '~icons/tabler/circle-plus'
-export { default as Edit } from '~icons/tabler/pencil'
+export { default as Edit } from '~icons/mdi/edit'
+export { default as EditOff } from '~icons/mdi/edit-off'
 export { default as Delete } from '~icons/tabler/trash'
 
 export { default as Network } from '~icons/tabler/router'

app/src/lib/components/menu/Menu.svelte

@@ -19,7 +19,8 @@
     Router,
     AP,
     Copyright,
-    Metrics
+    Metrics,
+    DNS
   } from '$lib/components/icons'
   import appEnv from 'app-env'
 
@@ -103,6 +104,12 @@
             icon: AP,
             href: '/wifi/ap',
             feature: true
+          },
+          {
+            title: 'mDNS',
+            icon: DNS,
+            href: '/wifi/mdns',
+            feature: true
           }
         ]
       },
@@ -127,7 +134,7 @@
             title: 'System Metrics',
             icon: Metrics,
             href: '/system/metrics',
-            feature: $features.analytics
+            feature: true
           },
           {
             title: 'Firmware Update',
@@ -165,7 +172,11 @@
 <div class="flex h-full w-80 flex-col p-4 bg-base-200 text-base-content">
   <LogoButton {appName} />
 
-  <MenuList {menuItems} select={updateMenu} class="grow flex-nowrap overflow-y-auto" level="0" />
+  <MenuList
+    {menuItems}
+    select={updateMenu}
+    class="grow flex-nowrap overflow-y-auto overflow-x-hidden"
+    level="0" />
 
   <div class="divider my-0"></div>
 

app/src/lib/components/menu/MenuList.svelte

@@ -16,13 +16,13 @@
   }
 </script>
 
-<ul class={klass + ' menu'}>
+<ul class={klass + ' menu w-full'}>
   {#each menuItems as MenuItem[] as menuItem, i (menuItem.title)}
     {#if menuItem.feature}
       <li>
         {#if menuItem.submenu}
           <details open={menuItem.submenu.some(subItem => subItem.active)}>
-            <summary class="text-lg font-bold">
+            <summary class="font-bold">
               <menuItem.icon class="h-6 w-6" />
               {menuItem.title}
             </summary>

app/src/lib/gait.ts

@@ -1,452 +1,447 @@
-import type { body_state_t } from './kinematic';
-import { fromInt8 } from './utilities';
+import type { body_state_t } from './kinematic'
+import Kinematic from './kinematic'
+import { fromInt8 } from './utilities'
 
-const { sin } = Math;
+const { sin } = Math
 
 export interface gait_state_t {
-    step_height: number;
-    step_x: number;
-    step_z: number;
-    step_angle: number;
-    step_velocity: number;
-    step_depth: number;
+  step_height: number
+  step_x: number
+  step_z: number
+  step_angle: number
+  step_velocity: number
+  step_depth: number
 }
 
 export interface ControllerCommand {
-    stop: number;
-    lx: number;
-    ly: number;
-    rx: number;
-    ry: number;
-    h: number;
-    s: number;
-    s1: number;
+  stop: number
+  lx: number
+  ly: number
+  rx: number
+  ry: number
+  h: number
+  s: number
+  s1: number
 }
 
 export abstract class GaitState {
-    protected abstract name: string;
+  protected abstract name: string
 
-    protected dt = 0.02;
-    protected body_state!: body_state_t;
-    protected gait_state: gait_state_t = {
-        step_height: 0.4,
-        step_x: 0,
-        step_z: 0,
-        step_angle: 0,
-        step_velocity: 1,
-        step_depth: 0.002
-    };
+  protected dt = 0.02
+  protected body_state!: body_state_t
+  protected gait_state: gait_state_t = {
+    step_height: 0.4,
+    step_x: 0,
+    step_z: 0,
+    step_angle: 0,
+    step_velocity: 1,
+    step_depth: 0.002
+  }
 
-    public get default_feet_pos() {
-        return [
-            [1, -1, 1, 1],
-            [1, -1, -1, 1],
-            [-1, -1, 1, 1],
-            [-1, -1, -1, 1]
-        ];
+  public get default_feet_pos() {
+    return new Kinematic().getDefaultFeetPos()
+  }
+
+  protected get default_height() {
+    return 0.5
+  }
+
+  begin() {
+    console.log('Starting', this.name)
+  }
+  end() {
+    console.log('Ending', this.name)
+  }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    this.map_command(command)
+    this.body_state = body_state
+    this.dt = dt / 1000
+    return body_state
+  }
+
+  map_command(command: ControllerCommand) {
+    const newCommand = {
+      step_height: 0.4 + (command.s1 / 128 + 1) / 2,
+      step_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10,
+      step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10),
+      step_velocity: command.s / 128 + 1,
+      step_angle: command.rx / 128,
+      step_depth: 0.002
     }
 
-    protected get default_height() {
-        return 0.5;
-    }
-
-    begin() {
-        console.log('Starting', this.name);
-    }
-    end() {
-        console.log('Ending', this.name);
-    }
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        this.map_command(command);
-        this.body_state = body_state;
-        this.dt = dt / 1000;
-        return body_state;
-    }
-
-    map_command(command: ControllerCommand) {
-        const newCommand = {
-            step_height: 0.4 + (command.s1 / 128 + 1) / 2,
-            step_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10,
-            step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10),
-            step_velocity: command.s / 128 + 1,
-            step_angle: command.rx / 128,
-            step_depth: 0.002
-        };
-
-        this.gait_state = newCommand;
-    }
+    this.gait_state = newCommand
+  }
 }
 
 export class IdleState extends GaitState {
-    protected name = 'Idle';
+  protected name = 'Idle'
 }
 
 export class CalibrationState extends GaitState {
-    protected name = 'Calibration';
+  protected name = 'Calibration'
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        body_state.omega = 0;
-        body_state.phi = 0;
-        body_state.psi = 0;
-        body_state.xm = 0;
-        body_state.ym = this.default_height * 10;
-        body_state.zm = 0;
-        body_state.feet = this.default_feet_pos;
-        return body_state;
-    }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    body_state.omega = 0
+    body_state.phi = 0
+    body_state.psi = 0
+    body_state.xm = 0
+    body_state.ym = this.default_height * 10
+    body_state.zm = 0
+    body_state.feet = this.default_feet_pos
+    return body_state
+  }
 }
 
 export class RestState extends GaitState {
-    protected name = 'Rest';
+  protected name = 'Rest'
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        body_state.omega = 0;
-        body_state.phi = 0;
-        body_state.psi = 0;
-        body_state.xm = 0;
-        body_state.ym = this.default_height / 2;
-        body_state.zm = 0;
-        body_state.feet = this.default_feet_pos;
-        return body_state;
-    }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    body_state.omega = 0
+    body_state.phi = 0
+    body_state.psi = 0
+    body_state.xm = 0
+    body_state.ym = this.default_height / 2
+    body_state.zm = 0
+    body_state.feet = this.default_feet_pos
+    return body_state
+  }
 }
 
 export class StandState extends GaitState {
-    protected name = 'Stand';
+  protected name = 'Stand'
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        body_state.omega = 0;
-        body_state.phi = command.rx / 8;
-        body_state.psi = command.ry / 8;
-        body_state.xm = command.ly / 2 / 100;
-        body_state.zm = command.lx / 2 / 100;
-        body_state.feet = this.default_feet_pos;
-        return body_state;
-    }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    body_state.omega = 0
+    body_state.phi = command.rx / 8
+    body_state.psi = command.ry / 8
+    body_state.xm = command.ly / 2 / 100
+    body_state.zm = command.lx / 2 / 100
+    body_state.feet = this.default_feet_pos
+    return body_state
+  }
 }
 
 abstract class PhaseGaitState extends GaitState {
-    protected tick = 0;
-    protected phase = 0;
-    protected phase_time = 0;
-    protected abstract num_phases: number;
-    protected abstract phase_speed_factor: number;
-    protected abstract swing_stand_ratio: number;
+  protected tick = 0
+  protected phase = 0
+  protected phase_time = 0
+  protected abstract num_phases: number
+  protected abstract phase_speed_factor: number
+  protected abstract swing_stand_ratio: number
 
-    protected contact_phases!: number[][];
-    protected shifts!: number[][];
+  protected contact_phases!: number[][]
+  protected shifts!: number[][]
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        super.step(body_state, command, dt);
-        this.update_phase();
-        this.update_body_position();
-        this.update_feet_positions();
-        return this.body_state;
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    super.step(body_state, command, dt)
+    this.update_phase()
+    this.update_body_position()
+    this.update_feet_positions()
+    return this.body_state
+  }
+
+  update_phase() {
+    this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity
+
+    if (this.phase_time >= 1) {
+      this.phase += 1
+      if (this.phase == this.num_phases) this.phase = 0
+      this.phase_time = 0
+    }
+  }
+
+  update_body_position() {
+    if (this.num_phases === 4) return
+
+    const shift = this.shifts[Math.floor(this.phase / 2)]
+
+    this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4
+    this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4
+  }
+
+  update_feet_positions() {
+    for (let i = 0; i < 4; i++) {
+      this.body_state.feet[i] = this.update_foot_position(i)
+    }
+  }
+
+  update_foot_position(index: number): number[] {
+    const contact = this.contact_phases[index][this.phase]
+    return contact ? this.stand(index) : this.swing(index)
+  }
+
+  stand(index: number): number[] {
+    const delta_pos = [
+      -this.gait_state.step_x * this.dt * this.swing_stand_ratio,
+      0,
+      -this.gait_state.step_z * this.dt * this.swing_stand_ratio
+    ]
+
+    this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
+    this.body_state.feet[index][1] = this.default_feet_pos[index][1]
+    this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
+    return this.body_state.feet[index]
+  }
+
+  swing(index: number): number[] {
+    const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt]
+
+    if (this.gait_state.step_x == 0) {
+      delta_pos[0] =
+        (this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8
     }
 
-    update_phase() {
-        this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity;
-
-        if (this.phase_time >= 1) {
-            this.phase += 1;
-            if (this.phase == this.num_phases) this.phase = 0;
-            this.phase_time = 0;
-        }
+    if (this.gait_state.step_z == 0) {
+      delta_pos[2] =
+        (this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8
     }
 
-    update_body_position() {
-        if (this.num_phases === 4) return;
-
-        const shift = this.shifts[Math.floor(this.phase / 2)];
-
-        this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4;
-        this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4;
-    }
-
-    update_feet_positions() {
-        for (let i = 0; i < 4; i++) {
-            this.body_state.feet[i] = this.update_foot_position(i);
-        }
-    }
-
-    update_foot_position(index: number): number[] {
-        const contact = this.contact_phases[index][this.phase];
-        return contact ? this.stand(index) : this.swing(index);
-    }
-
-    stand(index: number): number[] {
-        const delta_pos = [
-            -this.gait_state.step_x * this.dt * this.swing_stand_ratio,
-            0,
-            -this.gait_state.step_z * this.dt * this.swing_stand_ratio
-        ];
-
-        this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0];
-        this.body_state.feet[index][1] = this.default_feet_pos[index][1];
-        this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2];
-        return this.body_state.feet[index];
-    }
-
-    swing(index: number): number[] {
-        const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt];
-
-        if (this.gait_state.step_x == 0) {
-            delta_pos[0] =
-                (this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8;
-        }
-
-        if (this.gait_state.step_z == 0) {
-            delta_pos[2] =
-                (this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8;
-        }
-
-        this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0];
-        this.body_state.feet[index][1] =
-            this.default_feet_pos[index][1] +
-            sin(this.phase_time * Math.PI) * this.gait_state.step_height;
-        this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2];
-        return this.body_state.feet[index];
-    }
+    this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
+    this.body_state.feet[index][1] =
+      this.default_feet_pos[index][1] + sin(this.phase_time * Math.PI) * this.gait_state.step_height
+    this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
+    return this.body_state.feet[index]
+  }
 }
 
 export class FourPhaseWalkState extends PhaseGaitState {
-    protected name = 'Four phase walk';
-    protected num_phases = 4;
-    protected phase_speed_factor = 6;
-    protected contact_phases = [
-        [1, 0, 1, 1],
-        [1, 1, 1, 0],
-        [1, 1, 1, 0],
-        [1, 0, 1, 1]
-    ];
-    protected swing_stand_ratio = 1 / (this.num_phases - 1);
+  protected name = 'Four phase walk'
+  protected num_phases = 4
+  protected phase_speed_factor = 6
+  protected contact_phases = [
+    [1, 0, 1, 1],
+    [1, 1, 1, 0],
+    [1, 1, 1, 0],
+    [1, 0, 1, 1]
+  ]
+  protected swing_stand_ratio = 1 / (this.num_phases - 1)
 
-    begin() {
-        super.begin();
-    }
+  begin() {
+    super.begin()
+  }
 
-    end() {
-        super.end();
-    }
+  end() {
+    super.end()
+  }
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        return super.step(body_state, command, dt);
-    }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    return super.step(body_state, command, dt)
+  }
 }
 
 export class EightPhaseWalkState extends PhaseGaitState {
-    protected name = 'Eight phase walk';
-    protected num_phases = 8;
-    protected phase_speed_factor = 4;
-    protected contact_phases = [
-        [1, 0, 1, 1, 1, 1, 1, 1],
-        [1, 1, 1, 1, 1, 0, 1, 1],
-        [1, 1, 1, 1, 1, 1, 1, 0],
-        [1, 1, 1, 0, 1, 1, 1, 1]
-    ];
-    protected shifts = [
-        [-0.05, 0, -0.2],
-        [0.3, 0, 0.2],
-        [-0.05, 0, 0.2],
-        [0.3, 0, -0.2]
-    ];
-    protected swing_stand_ratio = 1 / (this.num_phases - 1);
+  protected name = 'Eight phase walk'
+  protected num_phases = 8
+  protected phase_speed_factor = 4
+  protected contact_phases = [
+    [1, 0, 1, 1, 1, 1, 1, 1],
+    [1, 1, 1, 1, 1, 0, 1, 1],
+    [1, 1, 1, 1, 1, 1, 1, 0],
+    [1, 1, 1, 0, 1, 1, 1, 1]
+  ]
+  protected shifts = [
+    [-0.05, 0, -0.2],
+    [0.3, 0, 0.2],
+    [-0.05, 0, 0.2],
+    [0.3, 0, -0.2]
+  ]
+  protected swing_stand_ratio = 1 / (this.num_phases - 1)
 
-    begin() {
-        super.begin();
-    }
+  begin() {
+    super.begin()
+  }
 
-    end() {
-        super.end();
-    }
+  end() {
+    super.end()
+  }
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        return super.step(body_state, command, dt);
-    }
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    return super.step(body_state, command, dt)
+  }
 }
 
 export class BezierState extends GaitState {
-    protected name = 'Bezier';
-    protected phase = 0;
-    protected phase_num = 0;
-    protected step_length: number = 0;
-    offset = [0, 0.5, 0.5, 0];
+  protected name = 'Bezier'
+  protected phase = 0
+  protected phase_num = 0
+  protected step_length: number = 0
+  offset = [0, 0.5, 0.5, 0]
 
-    begin() {
-        super.begin();
+  begin() {
+    super.begin()
+  }
+
+  end() {
+    super.end()
+  }
+
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    super.step(body_state, command, dt)
+    this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2)
+    if (this.gait_state.step_x < 0) {
+      this.step_length = -this.step_length
     }
+    this.update_phase()
+    this.update_feet_positions()
+    return this.body_state
+  }
 
-    end() {
-        super.end();
+  update_phase() {
+    this.phase += this.dt * this.gait_state.step_velocity * 2
+    if (this.phase >= 1) {
+      this.phase_num += 1
+      this.phase_num %= 2
+      this.phase = 0
     }
+  }
 
-    step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-        super.step(body_state, command, dt);
-        this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2);
-        if (this.gait_state.step_x < 0) {
-            this.step_length = -this.step_length;
-        }
-        this.update_phase();
-        this.update_feet_positions();
-        return this.body_state;
+  update_feet_positions() {
+    for (let i = 0; i < 4; i++) {
+      this.body_state.feet[i] = this.update_foot_position(i)
     }
+  }
 
-    update_phase() {
-        this.phase += this.dt * this.gait_state.step_velocity * 2;
-        if (this.phase >= 1) {
-            this.phase_num += 1;
-            this.phase_num %= 2;
-            this.phase = 0;
-        }
+  update_foot_position(index: number): number[] {
+    let phase = this.phase + this.offset[index]
+    if (phase >= 1) {
+      phase -= 1
     }
+    this.body_state.feet[index][0] = this.default_feet_pos[index][0]
+    this.body_state.feet[index][1] = this.default_feet_pos[index][1]
+    this.body_state.feet[index][2] = this.default_feet_pos[index][2]
+    return phase <= 0.75 ?
+        this.stand_controller(index, phase / 0.75)
+      : this.swing_controller(index, (phase - 0.75) / (1 - 0.75))
+  }
 
-    update_feet_positions() {
-        for (let i = 0; i < 4; i++) {
-            this.body_state.feet[i] = this.update_foot_position(i);
-        }
-    }
+  stand_controller(index: number, phase: number) {
+    let depth = this.gait_state.step_depth
+    return this.controller(index, phase, stance_curve, depth)
+  }
 
-    update_foot_position(index: number): number[] {
-        let phase = this.phase + this.offset[index];
-        if (phase >= 1) {
-            phase -= 1;
-        }
-        this.body_state.feet[index][0] = this.default_feet_pos[index][0];
-        this.body_state.feet[index][1] = this.default_feet_pos[index][1];
-        this.body_state.feet[index][2] = this.default_feet_pos[index][2];
-        return phase <= 0.75 ?
-                this.stand_controller(index, phase / 0.75)
-            :   this.swing_controller(index, (phase - 0.75) / (1 - 0.75));
-    }
+  swing_controller(index: number, phase: number) {
+    let height = this.gait_state.step_height
+    return this.controller(index, phase, bezier_curve, height)
+  }
 
-    stand_controller(index: number, phase: number) {
-        let depth = this.gait_state.step_depth;
-        return this.controller(index, phase, stance_curve, depth);
-    }
+  controller(
+    index: number,
+    phase: number,
+    controller: (length: number, angle: number, ...args: number[]) => number[],
+    ...args: number[]
+  ) {
+    let length = this.step_length / 2
+    let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2
+    const delta_pos = controller(length, angle, ...args, phase)
 
-    swing_controller(index: number, phase: number) {
-        let height = this.gait_state.step_height;
-        return this.controller(index, phase, bezier_curve, height);
-    }
+    length = this.gait_state.step_angle * 2
+    angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])
 
-    controller(
-        index: number,
-        phase: number,
-        controller: (length: number, angle: number, ...args: number[]) => number[],
-        ...args: number[]
-    ) {
-        let length = this.step_length / 2;
-        let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2;
-        const delta_pos = controller(length, angle, ...args, phase);
+    const delta_rot = controller(length, angle, ...args, phase)
 
-        length = this.gait_state.step_angle * 2;
-        angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index]);
+    this.body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2
+    this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2
+    if (this.gait_state.step_x || this.gait_state.step_z || this.gait_state.step_angle)
+      this.body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2
 
-        const delta_rot = controller(length, angle, ...args, phase);
-
-        this.body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2;
-        this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2;
-        if (this.gait_state.step_x || this.gait_state.step_z || this.gait_state.step_angle)
-            this.body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2;
-
-        return this.body_state.feet[index];
-    }
+    return this.body_state.feet[index]
+  }
 }
 
 const stance_curve = (length: number, angle: number, depth: number, phase: number): number[] => {
-    const X_POLAR = Math.cos(angle);
-    const Y_POLAR = Math.sin(angle);
+  const X_POLAR = Math.cos(angle)
+  const Y_POLAR = Math.sin(angle)
 
-    const step = length * (1 - 2 * phase);
-    const X = step * X_POLAR;
-    const Z = step * Y_POLAR;
-    let Y = 0;
+  const step = length * (1 - 2 * phase)
+  const X = step * X_POLAR
+  const Z = step * Y_POLAR
+  let Y = 0
 
-    if (length !== 0) {
-        Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length));
-    }
-    return [X, Y, Z];
-};
+  if (length !== 0) {
+    Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length))
+  }
+  return [X, Y, Z]
+}
 
 const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => {
-    const foot_mag = Math.sqrt(default_foot_pos[0] ** 2 + default_foot_pos[2] ** 2);
-    const foot_dir = Math.atan2(default_foot_pos[2], default_foot_pos[0]);
-    const offsets = [
-        current_foot_pos[0] - default_foot_pos[0],
-        current_foot_pos[2] - default_foot_pos[2],
-        current_foot_pos[1] - default_foot_pos[1]
-    ];
-    const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2);
-    const offset_mod = Math.atan2(offset_mag, foot_mag);
+  const foot_mag = Math.sqrt(default_foot_pos[0] ** 2 + default_foot_pos[2] ** 2)
+  const foot_dir = Math.atan2(default_foot_pos[2], default_foot_pos[0])
+  const offsets = [
+    current_foot_pos[0] - default_foot_pos[0],
+    current_foot_pos[2] - default_foot_pos[2],
+    current_foot_pos[1] - default_foot_pos[1]
+  ]
+  const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2)
+  const offset_mod = Math.atan2(offset_mag, foot_mag)
 
-    return Math.PI / 2.0 + foot_dir + offset_mod;
-};
+  return Math.PI / 2.0 + foot_dir + offset_mod
+}
 
 const bezier_curve = (length: number, angle: number, height: number, phase: number): number[] => {
-    const control_points = get_control_points(length, angle, height);
-    const n = control_points.length - 1;
+  const control_points = get_control_points(length, angle, height)
+  const n = control_points.length - 1
 
-    const point = [0, 0, 0];
-    for (let i = 0; i <= n; i++) {
-        const bernstein_poly = comb(n, i) * Math.pow(phase, i) * Math.pow(1 - phase, n - i);
-        point[0] += bernstein_poly * control_points[i][0];
-        point[1] += bernstein_poly * control_points[i][1];
-        point[2] += bernstein_poly * control_points[i][2];
-    }
-    return point;
-};
+  const point = [0, 0, 0]
+  for (let i = 0; i <= n; i++) {
+    const bernstein_poly = comb(n, i) * Math.pow(phase, i) * Math.pow(1 - phase, n - i)
+    point[0] += bernstein_poly * control_points[i][0]
+    point[1] += bernstein_poly * control_points[i][1]
+    point[2] += bernstein_poly * control_points[i][2]
+  }
+  return point
+}
 const get_control_points = (length: number, angle: number, height: number): number[][] => {
-    const X_POLAR = Math.cos(angle);
-    const Z_POLAR = Math.sin(angle);
+  const X_POLAR = Math.cos(angle)
+  const Z_POLAR = Math.sin(angle)
 
-    const STEP = [
-        -length,
-        -length * 1.4,
-        -length * 1.5,
-        -length * 1.5,
-        -length * 1.5,
-        0.0,
-        0.0,
-        0.0,
-        length * 1.5,
-        length * 1.5,
-        length * 1.4,
-        length
-    ];
+  const STEP = [
+    -length,
+    -length * 1.4,
+    -length * 1.5,
+    -length * 1.5,
+    -length * 1.5,
+    0.0,
+    0.0,
+    0.0,
+    length * 1.5,
+    length * 1.5,
+    length * 1.4,
+    length
+  ]
 
-    const Y = [
-        0.0,
-        0.0,
-        height * 0.9,
-        height * 0.9,
-        height * 0.9,
-        height * 0.9,
-        height * 0.9,
-        height * 1.1,
-        height * 1.1,
-        height * 1.1,
-        0.0,
-        0.0
-    ];
+  const Y = [
+    0.0,
+    0.0,
+    height * 0.9,
+    height * 0.9,
+    height * 0.9,
+    height * 0.9,
+    height * 0.9,
+    height * 1.1,
+    height * 1.1,
+    height * 1.1,
+    0.0,
+    0.0
+  ]
 
-    const control_points: number[][] = [];
+  const control_points: number[][] = []
 
-    for (let i = 0; i < STEP.length; i++) {
-        const X = STEP[i] * X_POLAR;
-        const Z = STEP[i] * Z_POLAR;
-        control_points.push([X, Y[i], Z]);
-    }
+  for (let i = 0; i < STEP.length; i++) {
+    const X = STEP[i] * X_POLAR
+    const Z = STEP[i] * Z_POLAR
+    control_points.push([X, Y[i], Z])
+  }
 
-    return control_points;
-};
+  return control_points
+}
 
 const comb = (n: number, k: number): number => {
-    if (k < 0 || k > n) return 0;
-    if (k === 0 || k === n) return 1;
-    k = Math.min(k, n - k);
-    let c = 1;
-    for (let i = 0; i < k; i++) {
-        c = (c * (n - i)) / (i + 1);
-    }
-    return c;
-};
+  if (k < 0 || k > n) return 0
+  if (k === 0 || k === n) return 1
+  k = Math.min(k, n - k)
+  let c = 1
+  for (let i = 0; i < k; i++) {
+    c = (c * (n - i)) / (i + 1)
+  }
+  return c
+}

app/src/lib/kinematic.ts

@@ -1,320 +1,131 @@
-
 export interface body_state_t {
-	omega: number;
-	phi: number;
-	psi: number;
-	xm: number;
-	ym: number;
-	zm: number;
-	feet: number[][];
+  omega: number
+  phi: number
+  psi: number
+  xm: number
+  ym: number
+  zm: number
+  feet: number[][]
 }
 
 export interface position {
-	x: number;
-	y: number;
-	z: number;
+  x: number
+  y: number
+  z: number
 }
 
 export interface target_position {
-	x: number;
-	z: number;
-	yaw: number;
+  x: number
+  z: number
+  yaw: number
 }
 
-const { cos, sin, atan2, sqrt } = Math;
+const { cos, sin, atan2, acos, sqrt, max, min } = Math
 
-const DEG2RAD = 0.017453292519943;
+const DEG2RAD = 0.017453292519943
 
 export default class Kinematic {
-	l1: number;
-	l2: number;
-	l3: number;
-	l4: number;
-
-	L: number;
-	W: number;
-
-	DEG2RAD = DEG2RAD;
-
-	sHp = sin(Math.PI / 2);
-	cHp = cos(Math.PI / 2);
-
-	Tlf: number[][] = [];
-	Trf: number[][] = [];
-	Tlb: number[][] = [];
-	Trb: number[][] = [];
-
-	point_lf: number[][];
-	point_rf: number[][];
-	point_lb: number[][];
-	point_rb: number[][];
-	Ix: number[][];
-
-	constructor() {
-		this.l1 = 60.5 / 100;
-		this.l2 = 10 / 100;
-		this.l3 = 100.7 / 100;
-		this.l4 = 118.5 / 100;
-
-		this.L = 207.5 / 100;
-		this.W = 78 / 100;
-
-		this.point_lf = [
-			[this.cHp, 0, this.sHp, this.L / 2],
-			[0, 1, 0, 0],
-			[-this.sHp, 0, this.cHp, this.W / 2],
-			[0, 0, 0, 1]
-		];
-
-		this.point_rf = [
-			[this.cHp, 0, this.sHp, this.L / 2],
-			[0, 1, 0, 0],
-			[-this.sHp, 0, this.cHp, -this.W / 2],
-			[0, 0, 0, 1]
-		];
-
-		this.point_lb = [
-			[this.cHp, 0, this.sHp, -this.L / 2],
-			[0, 1, 0, 0],
-			[-this.sHp, 0, this.cHp, this.W / 2],
-			[0, 0, 0, 1]
-		];
-
-		this.point_rb = [
-			[this.cHp, 0, this.sHp, -this.L / 2],
-			[0, 1, 0, 0],
-			[-this.sHp, 0, this.cHp, -this.W / 2],
-			[0, 0, 0, 1]
-		];
-		this.Ix = [
-			[-1, 0, 0, 0],
-			[0, 1, 0, 0],
-			[0, 0, 1, 0],
-			[0, 0, 0, 1]
-		];
-	}
-
-	public calcIK(body_state: body_state_t): number[] {
-		this.bodyIK(body_state);
-
-		return [
-			...this.legIK(this.multiplyVector(this.inverse(this.Tlf), body_state.feet[0])),
-			...this.legIK(
-				this.multiplyVector(
-					this.Ix,
-					this.multiplyVector(this.inverse(this.Trf), body_state.feet[1])
-				)
-			),
-			...this.legIK(this.multiplyVector(this.inverse(this.Tlb), body_state.feet[2])),
-			...this.legIK(
-				this.multiplyVector(
-					this.Ix,
-					this.multiplyVector(this.inverse(this.Trb), body_state.feet[3])
-				)
-			)
-		];
-	}
-
-	bodyIK(p: body_state_t) {
-		const cos_omega = cos(p.omega * this.DEG2RAD);
-		const sin_omega = sin(p.omega * this.DEG2RAD);
-		const cos_phi = cos(p.phi * this.DEG2RAD);
-		const sin_phi = sin(p.phi * this.DEG2RAD);
-		const cos_psi = cos(p.psi * this.DEG2RAD);
-		const sin_psi = sin(p.psi * this.DEG2RAD);
-
-		const Tm: number[][] = [
-			[cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm],
-			[
-				sin_omega * sin_phi * cos_psi + sin_psi * cos_omega,
-				-sin_omega * sin_phi * sin_psi + cos_omega * cos_psi,
-				-sin_omega * cos_phi,
-				p.ym
-			],
-			[
-				sin_omega * sin_psi - sin_phi * cos_omega * cos_psi,
-				sin_omega * cos_psi + sin_phi * sin_psi * cos_omega,
-				cos_omega * cos_phi,
-				p.zm
-			],
-			[0, 0, 0, 1]
-		];
-
-		this.Tlf = this.matrixMultiply(Tm, this.point_lf);
-		this.Trf = this.matrixMultiply(Tm, this.point_rf);
-		this.Tlb = this.matrixMultiply(Tm, this.point_lb);
-		this.Trb = this.matrixMultiply(Tm, this.point_rb);
-	}
-
-	public legIK(point: number[]): number[] {
-		const [x, y, z] = point;
-
-		let F = sqrt(x ** 2 + y ** 2 - this.l1 ** 2);
-		if (isNaN(F)) F = this.l1;
-
-		const G = F - this.l2;
-		const H = sqrt(G ** 2 + z ** 2);
-
-		const theta1 = -atan2(y, x) - atan2(F, -this.l1);
-		const D = (H ** 2 - this.l3 ** 2 - this.l4 ** 2) / (2 * this.l3 * this.l4);
-		let theta3 = atan2(sqrt(1 - D ** 2), D);
-		if (isNaN(theta3)) theta3 = 0;
-
-		const theta2 = atan2(z, G) - atan2(this.l4 * sin(theta3), this.l3 + this.l4 * cos(theta3));
-
-		return [theta1, theta2, theta3];
-	}
-
-	matrixMultiply(a: number[][], b: number[][]): number[][] {
-		const result: number[][] = [];
-
-		for (let i = 0; i < a.length; i++) {
-			const row: number[] = [];
-
-			for (let j = 0; j < b[0].length; j++) {
-				let sum = 0;
-
-				for (let k = 0; k < a[i].length; k++) {
-					sum += a[i][k] * b[k][j];
-				}
-
-				row.push(sum);
-			}
-
-			result.push(row);
-		}
-
-		return result;
-	}
-
-	multiplyVector(matrix: number[][], vector: number[]): number[] {
-		const rows = matrix.length;
-		const cols = matrix[0].length;
-		const vectorLength = vector.length;
-
-		if (cols !== vectorLength) {
-			throw new Error('Matrix and vector dimensions do not match for multiplication.');
-		}
-
-		const result = [];
-
-		for (let i = 0; i < rows; i++) {
-			let sum = 0;
-
-			for (let j = 0; j < cols; j++) {
-				sum += matrix[i][j] * vector[j];
-			}
-
-			result.push(sum);
-		}
-
-		return result;
-	}
-
-	private inverse(matrix: number[][]): number[][] {
-		const det = this.determinant(matrix);
-		const adjugate = this.adjugate(matrix);
-		const scalar = 1 / det;
-		const inverse: number[][] = [];
-
-		for (let i = 0; i < matrix.length; i++) {
-			const row: number[] = [];
-
-			for (let j = 0; j < matrix[i].length; j++) {
-				row.push(adjugate[i][j] * scalar);
-			}
-
-			inverse.push(row);
-		}
-
-		return inverse;
-	}
-
-	private determinant(matrix: number[][]): number {
-		if (matrix.length !== matrix[0].length) {
-			throw new Error('The matrix is not square.');
-		}
-
-		if (matrix.length === 2) {
-			return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
-		}
-
-		let det = 0;
-
-		for (let i = 0; i < matrix.length; i++) {
-			const sign = i % 2 === 0 ? 1 : -1;
-			const subMatrix: number[][] = [];
-
-			for (let j = 1; j < matrix.length; j++) {
-				const row: number[] = [];
-
-				for (let k = 0; k < matrix.length; k++) {
-					if (k !== i) {
-						row.push(matrix[j][k]);
-					}
-				}
-
-				subMatrix.push(row);
-			}
-
-			det += sign * matrix[0][i] * this.determinant(subMatrix);
-		}
-
-		return det;
-	}
-
-	private adjugate(matrix: number[][]): number[][] {
-		if (matrix.length !== matrix[0].length) {
-			throw new Error('The matrix is not square.');
-		}
-
-		const adjugate: number[][] = [];
-
-		for (let i = 0; i < matrix.length; i++) {
-			const row: number[] = [];
-
-			for (let j = 0; j < matrix[i].length; j++) {
-				const sign = (i + j) % 2 === 0 ? 1 : -1;
-				const subMatrix: number[][] = [];
-
-				for (let k = 0; k < matrix.length; k++) {
-					if (k !== i) {
-						const subRow: number[] = [];
-
-						for (let l = 0; l < matrix.length; l++) {
-							if (l !== j) {
-								subRow.push(matrix[k][l]);
-							}
-						}
-
-						subMatrix.push(subRow);
-					}
-				}
-
-				const cofactor = sign * this.determinant(subMatrix);
-				row.push(cofactor);
-			}
-
-			adjugate.push(row);
-		}
-
-		return this.transpose(adjugate);
-	}
-
-	private transpose(matrix: number[][]): number[][] {
-		const transposed: number[][] = [];
-
-		for (let i = 0; i < matrix.length; i++) {
-			const row: number[] = [];
-
-			for (let j = 0; j < matrix[i].length; j++) {
-				row.push(matrix[j][i]);
-			}
-
-			transposed.push(row);
-		}
-
-		return transposed;
-	}
+  l1: number
+  l2: number
+  l3: number
+  l4: number
+
+  L: number
+  W: number
+
+  DEG2RAD = DEG2RAD
+
+  mountOffsets: number[][]
+
+  invMountRot = [
+    [0, 0, -1],
+    [0, 1, 0],
+    [1, 0, 0]
+  ]
+
+  constructor() {
+    this.l1 = 60.5 / 100
+    this.l2 = 10 / 100
+    this.l3 = 111.7 / 100
+    this.l4 = 118.5 / 100
+
+    this.L = 207.5 / 100
+    this.W = 78 / 100
+
+    this.mountOffsets = [
+      [this.L / 2, 0, this.W / 2],
+      [this.L / 2, 0, -this.W / 2],
+      [-this.L / 2, 0, this.W / 2],
+      [-this.L / 2, 0, -this.W / 2]
+    ]
+  }
+
+  getDefaultFeetPos(): number[][] {
+    return this.mountOffsets.map((offset, i) => {
+      return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.l1 : this.l1)]
+    })
+  }
+
+  calcIK(p: body_state_t): number[] {
+    const roll = p.omega * this.DEG2RAD
+    const pitch = p.phi * this.DEG2RAD
+    const yaw = p.psi * this.DEG2RAD
+    const rot = this.euler2R(roll, pitch, yaw)
+    const inv_rot = [
+      [rot[0][0], rot[1][0], rot[2][0]],
+      [rot[0][1], rot[1][1], rot[2][1]],
+      [rot[0][2], rot[1][2], rot[2][2]]
+    ]
+    const inv_trans = [
+      -inv_rot[0][0] * p.xm - inv_rot[0][1] * p.ym - inv_rot[0][2] * p.zm,
+      -inv_rot[1][0] * p.xm - inv_rot[1][1] * p.ym - inv_rot[1][2] * p.zm,
+      -inv_rot[2][0] * p.xm - inv_rot[2][1] * p.ym - inv_rot[2][2] * p.zm
+    ]
+    return p.feet.flatMap((foot, i) => {
+      const [wx, wy, wz] = foot
+      const bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0]
+      const by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1]
+      const bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2]
+
+      const [mx, my, mz] = this.mountOffsets[i]
+      const px = bx - mx,
+        py = by - my,
+        pz = bz - mz
+
+      const lx =
+        this.invMountRot[0][0] * px + this.invMountRot[0][1] * py + this.invMountRot[0][2] * pz
+      const ly =
+        this.invMountRot[1][0] * px + this.invMountRot[1][1] * py + this.invMountRot[1][2] * pz
+      const lz =
+        this.invMountRot[2][0] * px + this.invMountRot[2][1] * py + this.invMountRot[2][2] * pz
+
+      const xLocal = i % 2 === 1 ? -lx : lx
+      return this.legIK(xLocal, ly, lz)
+    })
+  }
+
+  private legIK(x: number, y: number, z: number): [number, number, number] {
+    const F = sqrt(max(0, x * x + y * y - this.l1 * this.l1))
+    const G = F - this.l2
+    const H = sqrt(G * G + z * z)
+    const t1 = -atan2(y, x) - atan2(F, -this.l1)
+    const D = (H * H - this.l3 * this.l3 - this.l4 * this.l4) / (2 * this.l3 * this.l4)
+    const t3 = acos(max(-1, min(1, D)))
+    const t2 = atan2(z, G) - atan2(this.l4 * sin(t3), this.l3 + this.l4 * cos(t3))
+    return [t1, t2, t3]
+  }
+
+  private euler2R(roll: number, pitch: number, yaw: number): number[][] {
+    const cr = cos(roll),
+      sr = sin(roll)
+    const cp = cos(pitch),
+      sp = sin(pitch)
+    const cy = cos(yaw),
+      sy = sin(yaw)
+    return [
+      [cp * cy, -cp * sy, sp],
+      [sr * sp * cy + sy * cr, -sr * sp * sy + cr * cy, -sr * cp],
+      [sr * sy - sp * cr * cy, sr * cy + sp * sy * cr, cr * cp]
+    ]
+  }
 }
-

app/src/lib/sceneBuilder.ts

@@ -1,379 +1,379 @@
 import {
-  Mesh,
-  PerspectiveCamera,
-  PlaneGeometry,
-  Scene,
-  WebGLRenderer,
-  AmbientLight,
-  DirectionalLight,
-  PCFSoftShadowMap,
-  type GridHelper,
-  ArrowHelper,
-  Vector3,
-  FogExp2,
-  CanvasTexture,
-  type ColorRepresentation,
-  type WebGLRendererParameters,
-  MeshPhongMaterial,
-  EquirectangularReflectionMapping,
-  ACESFilmicToneMapping,
-  MathUtils,
-  Group,
-  MeshBasicMaterial,
-  RepeatWrapping
-} from 'three'
-import { Sky } from 'three/addons/objects/Sky.js'
-import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'
-import { TransformControls } from 'three/examples/jsm/controls/TransformControls'
-import { Reflector } from 'three/examples/jsm/objects/Reflector.js'
-import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader'
-import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
-import { sunCalculator } from './utilities/position-utilities'
+    Mesh,
+    PerspectiveCamera,
+    PlaneGeometry,
+    Scene,
+    WebGLRenderer,
+    AmbientLight,
+    DirectionalLight,
+    PCFSoftShadowMap,
+    type GridHelper,
+    ArrowHelper,
+    Vector3,
+    FogExp2,
+    CanvasTexture,
+    type ColorRepresentation,
+    type WebGLRendererParameters,
+    MeshPhongMaterial,
+    EquirectangularReflectionMapping,
+    ACESFilmicToneMapping,
+    MathUtils,
+    Group,
+    MeshBasicMaterial,
+    RepeatWrapping
+} from 'three';
+import { Sky } from 'three/addons/objects/Sky.js';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
+import { TransformControls } from 'three/examples/jsm/controls/TransformControls';
+import { Reflector } from 'three/examples/jsm/objects/Reflector.js';
+import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader';
+import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls';
+import { sunCalculator } from './utilities/position-utilities';
 
-export const addScene = () => new Scene()
+export const addScene = () => new Scene();
 
 interface position {
-  x?: number
-  y?: number
-  z?: number
+    x?: number;
+    y?: number;
+    z?: number;
 }
 
 interface light {
-  color?: ColorRepresentation
-  intensity?: number
+    color?: ColorRepresentation;
+    intensity?: number;
 }
 
 interface arrowOptions {
-  origin: position
-  direction: position
-  length?: number
-  color?: ColorRepresentation
+    origin: position;
+    direction: position;
+    length?: number;
+    color?: ColorRepresentation;
 }
 
-type directionalLight = position & light
+type directionalLight = position & light;
 
 export default class SceneBuilder {
-  public scene: Scene
-  public camera!: PerspectiveCamera
-  public ground!: Mesh
-  public renderer!: WebGLRenderer
-  public orbit: OrbitControls
-  public callback: Function | undefined
-  public gridHelper!: GridHelper
-  public model!: URDFRobot
-  public liveStreamTexture!: CanvasTexture
-  private fog!: FogExp2
-  private isLoaded: boolean = false
-  public isDragging: boolean = false
-  highlightMaterial: any
-  sky!: Sky
-  transformControl: TransformControls
-  public modelGroup!: Group
+    public scene: Scene;
+    public camera!: PerspectiveCamera;
+    public ground!: Mesh;
+    public renderer!: WebGLRenderer;
+    public orbit: OrbitControls;
+    public callback: Function | undefined;
+    public gridHelper!: GridHelper;
+    public model!: URDFRobot;
+    public liveStreamTexture!: CanvasTexture;
+    private fog!: FogExp2;
+    private isLoaded: boolean = false;
+    public isDragging: boolean = false;
+    highlightMaterial: any;
+    sky!: Sky;
+    transformControl: TransformControls;
+    public modelGroup!: Group;
 
-  constructor() {
-    this.scene = new Scene()
-    if (this.scene.environment?.mapping) {
-      this.scene.environment.mapping = EquirectangularReflectionMapping
-    }
-    return this
-  }
-
-  public addRenderer = (parameters?: WebGLRendererParameters) => {
-    this.renderer = new WebGLRenderer(parameters)
-    this.renderer.outputColorSpace = 'srgb'
-    this.renderer.shadowMap.enabled = true
-    this.renderer.shadowMap.type = PCFSoftShadowMap
-    this.renderer.toneMapping = ACESFilmicToneMapping
-    this.renderer.toneMappingExposure = 0.85
-    if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement)
-    return this
-  }
-
-  public addSky = () => {
-    this.sky = new Sky()
-    this.sky.scale.setScalar(450000)
-    this.scene.add(this.sky)
-    const effectController = {
-      turbidity: 10,
-      rayleigh: 3,
-      mieCoefficient: 0.005,
-      mieDirectionalG: 0.7,
-      elevation: sunCalculator.calculateSunElevation(),
-      azimuth: 200,
-      exposure: this.renderer.toneMappingExposure
-    }
-    const uniforms = this.sky.material.uniforms
-    uniforms['turbidity'].value = effectController.turbidity
-    uniforms['rayleigh'].value = effectController.rayleigh
-    uniforms['mieCoefficient'].value = effectController.mieCoefficient
-    uniforms['mieDirectionalG'].value = effectController.mieDirectionalG
-    this.renderer.toneMappingExposure = 0.5
-    const phi = MathUtils.degToRad(90 - effectController.elevation)
-    const theta = MathUtils.degToRad(effectController.azimuth)
-    const sun = new Vector3()
-
-    sun.setFromSphericalCoords(1, phi, theta)
-    uniforms['sunPosition'].value.copy(sun)
-    return this
-  }
-
-  public addPerspectiveCamera = (options: position) => {
-    this.camera = new PerspectiveCamera()
-    this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0)
-    this.scene.add(this.camera)
-    return this
-  }
-
-  public addGroundPlane = (options?: position) => {
-    const checkerboardTexture = this.createCheckerboardTexture(1024, 2)
-    checkerboardTexture.wrapS = RepeatWrapping
-    checkerboardTexture.wrapT = RepeatWrapping
-    checkerboardTexture.repeat.set(100, 100)
-    const checkerboardMat = new MeshBasicMaterial({
-      map: checkerboardTexture,
-      opacity: 0.1,
-      transparent: true
-    })
-
-    const plane = new PlaneGeometry(400, 400)
-
-    this.ground = new Mesh(plane, checkerboardMat)
-    this.ground.rotation.x = -Math.PI / 2
-    this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0)
-    this.ground.receiveShadow = true
-    this.scene.add(this.ground)
-
-    const mirror = new Reflector(plane, {
-      clipBias: 0.003,
-      textureWidth: window.innerWidth * window.devicePixelRatio,
-      textureHeight: window.innerHeight * window.devicePixelRatio,
-      color: 0x00bfff
-    })
-    mirror.rotateX(-Math.PI / 2)
-    this.scene.add(mirror)
-
-    return this
-  }
-
-  public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
-    this.orbit = new OrbitControls(this.camera, this.renderer.domElement)
-    this.orbit.minDistance = 5
-    this.orbit.maxDistance = maxDistance
-    this.orbit.autoRotate = autoRotate
-    this.orbit.update()
-    return this
-  }
-
-  public addAmbientLight = (options: light) => {
-    const ambientLight = new AmbientLight(options.color, options.intensity)
-    this.scene.add(ambientLight)
-    return this
-  }
-
-  public addDirectionalLight = (options: directionalLight) => {
-    const directionalLight = new DirectionalLight(options.color, options.intensity)
-    directionalLight.castShadow = true
-    directionalLight.shadow.camera.top = 10
-    directionalLight.shadow.camera.bottom = -10
-    directionalLight.shadow.camera.right = 10
-    directionalLight.shadow.camera.left = -10
-    directionalLight.shadow.mapSize.set(4096, 4096)
-
-    directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0)
-    this.scene.add(directionalLight)
-    return this
-  }
-
-  private createCheckerboardTexture = (size: number, squares: number) => {
-    const canvas = document.createElement('canvas')
-    canvas.width = size
-    canvas.height = size
-    const context = canvas.getContext('2d')
-
-    const squareSize = size / squares
-
-    for (let y = 0; y < squares; y++) {
-      for (let x = 0; x < squares; x++) {
-        context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000'
-        context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize)
-      }
-    }
-
-    const texture = new CanvasTexture(canvas)
-    texture.wrapS = texture.wrapT = RepeatWrapping
-    texture.anisotropy = 16
-    return texture
-  }
-
-  public addFogExp2 = (color: ColorRepresentation, density?: number) => {
-    this.scene.fog = new FogExp2(color, density)
-    return this
-  }
-
-  public fillParent = () => {
-    const parentElement = this.renderer.domElement.parentElement
-    if (parentElement) {
-      const width = parentElement.clientWidth
-      const height = parentElement.clientHeight
-      this.handleResize(width, height)
-    }
-    return this
-  }
-
-  public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
-    this.renderer.setSize(width, height)
-    this.renderer.setPixelRatio(window.devicePixelRatio)
-    this.camera.aspect = width / height
-    this.camera.updateProjectionMatrix()
-    return this
-  }
-
-  public addRenderCb = (callback: Function) => {
-    this.callback = callback
-    return this
-  }
-
-  public startRenderLoop = () => {
-    this.renderer.setAnimationLoop(() => {
-      this.renderer.render(this.scene, this.camera)
-      this.orbit.update()
-      this.handleRobotShadow()
-      if (this.callback) this.callback()
-      if (!this.liveStreamTexture) return
-    })
-    return this
-  }
-
-  public addArrowHelper = (options?: arrowOptions) => {
-    const dir = new Vector3(
-      options?.direction.x ?? 0,
-      options?.direction.y ?? 0,
-      options?.direction.z ?? 0
-    )
-    const origin = new Vector3(
-      options?.origin.x ?? 0,
-      options?.origin.y ?? 0,
-      options?.origin.z ?? 0
-    )
-    const arrowHelper = new ArrowHelper(
-      dir,
-      origin,
-      options?.length ?? 1.5,
-      options?.color ?? 0xff0000
-    )
-    this.scene.add(arrowHelper)
-    return this
-  }
-
-  private setJointValue(jointName: string, angle: number) {
-    if (!this.model) return
-    if (!this.model.joints[jointName]) return
-    this.model.joints[jointName].setJointValue(angle)
-  }
-
-  isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed'
-
-  highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
-    const traverse = (c: any) => {
-      if (c.type === 'Mesh') {
-        if (revert) {
-          c.material = c.__origMaterial
-          delete c.__origMaterial
-        } else {
-          c.__origMaterial = c.material
-          c.material = material
+    constructor() {
+        this.scene = new Scene();
+        if (this.scene.environment?.mapping) {
+            this.scene.environment.mapping = EquirectangularReflectionMapping;
         }
-      }
+        return this;
+    }
 
-      if (c === m || !this.isJoint(c)) {
-        for (let i = 0; i < c.children.length; i++) {
-          const child = c.children[i]
-          if (!child.isURDFCollider) {
-            traverse(c.children[i])
-          }
+    public addRenderer = (parameters?: WebGLRendererParameters) => {
+        this.renderer = new WebGLRenderer(parameters);
+        this.renderer.outputColorSpace = 'srgb';
+        this.renderer.shadowMap.enabled = true;
+        this.renderer.shadowMap.type = PCFSoftShadowMap;
+        this.renderer.toneMapping = ACESFilmicToneMapping;
+        this.renderer.toneMappingExposure = 0.85;
+        if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement);
+        return this;
+    };
+
+    public addSky = () => {
+        this.sky = new Sky();
+        this.sky.scale.setScalar(450000);
+        this.scene.add(this.sky);
+        const effectController = {
+            turbidity: 10,
+            rayleigh: 3,
+            mieCoefficient: 0.005,
+            mieDirectionalG: 0.7,
+            elevation: sunCalculator.calculateSunElevation(),
+            azimuth: 200,
+            exposure: this.renderer.toneMappingExposure
+        };
+        const uniforms = this.sky.material.uniforms;
+        uniforms['turbidity'].value = effectController.turbidity;
+        uniforms['rayleigh'].value = effectController.rayleigh;
+        uniforms['mieCoefficient'].value = effectController.mieCoefficient;
+        uniforms['mieDirectionalG'].value = effectController.mieDirectionalG;
+        this.renderer.toneMappingExposure = 0.5;
+        const phi = MathUtils.degToRad(90 - effectController.elevation);
+        const theta = MathUtils.degToRad(effectController.azimuth);
+        const sun = new Vector3();
+
+        sun.setFromSphericalCoords(1, phi, theta);
+        uniforms['sunPosition'].value.copy(sun);
+        return this;
+    };
+
+    public addPerspectiveCamera = (options: position) => {
+        this.camera = new PerspectiveCamera();
+        this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0);
+        this.scene.add(this.camera);
+        return this;
+    };
+
+    public addGroundPlane = (options?: position) => {
+        const checkerboardTexture = this.createCheckerboardTexture(1024, 2);
+        checkerboardTexture.wrapS = RepeatWrapping;
+        checkerboardTexture.wrapT = RepeatWrapping;
+        checkerboardTexture.repeat.set(100, 100);
+        const checkerboardMat = new MeshBasicMaterial({
+            map: checkerboardTexture,
+            opacity: 0.1,
+            transparent: true
+        });
+
+        const plane = new PlaneGeometry(400, 400);
+
+        this.ground = new Mesh(plane, checkerboardMat);
+        this.ground.rotation.x = -Math.PI / 2;
+        this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0);
+        this.ground.receiveShadow = true;
+        this.scene.add(this.ground);
+
+        const mirror = new Reflector(plane, {
+            clipBias: 0.003,
+            textureWidth: window.innerWidth * window.devicePixelRatio,
+            textureHeight: window.innerHeight * window.devicePixelRatio,
+            color: 0x00bfff
+        });
+        mirror.rotateX(-Math.PI / 2);
+        this.scene.add(mirror);
+
+        return this;
+    };
+
+    public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
+        this.orbit = new OrbitControls(this.camera, this.renderer.domElement);
+        this.orbit.minDistance = minDistance;
+        this.orbit.maxDistance = maxDistance;
+        this.orbit.autoRotate = autoRotate;
+        this.orbit.update();
+        return this;
+    };
+
+    public addAmbientLight = (options: light) => {
+        const ambientLight = new AmbientLight(options.color, options.intensity);
+        this.scene.add(ambientLight);
+        return this;
+    };
+
+    public addDirectionalLight = (options: directionalLight) => {
+        const directionalLight = new DirectionalLight(options.color, options.intensity);
+        directionalLight.castShadow = true;
+        directionalLight.shadow.camera.top = 10;
+        directionalLight.shadow.camera.bottom = -10;
+        directionalLight.shadow.camera.right = 10;
+        directionalLight.shadow.camera.left = -10;
+        directionalLight.shadow.mapSize.set(4096, 4096);
+
+        directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0);
+        this.scene.add(directionalLight);
+        return this;
+    };
+
+    private createCheckerboardTexture = (size: number, squares: number) => {
+        const canvas = document.createElement('canvas');
+        canvas.width = size;
+        canvas.height = size;
+        const context = canvas.getContext('2d');
+
+        const squareSize = size / squares;
+
+        for (let y = 0; y < squares; y++) {
+            for (let x = 0; x < squares; x++) {
+                context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000';
+                context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize);
+            }
         }
-      }
+
+        const texture = new CanvasTexture(canvas);
+        texture.wrapS = texture.wrapT = RepeatWrapping;
+        texture.anisotropy = 16;
+        return texture;
+    };
+
+    public addFogExp2 = (color: ColorRepresentation, density?: number) => {
+        this.scene.fog = new FogExp2(color, density);
+        return this;
+    };
+
+    public fillParent = () => {
+        const parentElement = this.renderer.domElement.parentElement;
+        if (parentElement) {
+            const width = parentElement.clientWidth;
+            const height = parentElement.clientHeight;
+            this.handleResize(width, height);
+        }
+        return this;
+    };
+
+    public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
+        this.renderer.setSize(width, height);
+        this.renderer.setPixelRatio(window.devicePixelRatio);
+        this.camera.aspect = width / height;
+        this.camera.updateProjectionMatrix();
+        return this;
+    };
+
+    public addRenderCb = (callback: Function) => {
+        this.callback = callback;
+        return this;
+    };
+
+    public startRenderLoop = () => {
+        this.renderer.setAnimationLoop(() => {
+            this.renderer.render(this.scene, this.camera);
+            this.orbit.update();
+            this.handleRobotShadow();
+            if (this.callback) this.callback();
+            if (!this.liveStreamTexture) return;
+        });
+        return this;
+    };
+
+    public addArrowHelper = (options?: arrowOptions) => {
+        const dir = new Vector3(
+            options?.direction.x ?? 0,
+            options?.direction.y ?? 0,
+            options?.direction.z ?? 0
+        );
+        const origin = new Vector3(
+            options?.origin.x ?? 0,
+            options?.origin.y ?? 0,
+            options?.origin.z ?? 0
+        );
+        const arrowHelper = new ArrowHelper(
+            dir,
+            origin,
+            options?.length ?? 1.5,
+            options?.color ?? 0xff0000
+        );
+        this.scene.add(arrowHelper);
+        return this;
+    };
+
+    private setJointValue(jointName: string, angle: number) {
+        if (!this.model) return;
+        if (!this.model.joints[jointName]) return;
+        this.model.joints[jointName].setJointValue(angle);
     }
-    traverse(m)
-  }
 
-  public addTransformControls = (model: any) => {
-    this.transformControl = new TransformControls(this.camera, this.renderer.domElement)
-    this.transformControl.addEventListener('dragging-changed', (event: any) => {
-      this.orbit.enabled = !event.value
-      this.isDragging = !event.value
-    })
-    this.transformControl.attach(model)
-    this.scene.add(this.transformControl)
-    this.transformControl.setMode('rotate')
-    return this
-  }
+    isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed';
 
-  public addModel = (model: any) => {
-    this.modelGroup = new Group()
-    this.modelGroup.add(model)
-    this.model = model
-    this.scene.add(this.modelGroup)
-    return this
-  }
+    highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
+        const traverse = (c: any) => {
+            if (c.type === 'Mesh') {
+                if (revert) {
+                    c.material = c.__origMaterial;
+                    delete c.__origMaterial;
+                } else {
+                    c.__origMaterial = c.material;
+                    c.material = material;
+                }
+            }
 
-  public addDragControl = (updateAngle: any) => {
-    const highlightColor = '#FFFFFF'
-    const highlightMaterial = new MeshPhongMaterial({
-      shininess: 10,
-      color: highlightColor,
-      emissive: highlightColor,
-      emissiveIntensity: 0.9
-    })
+            if (c === m || !this.isJoint(c)) {
+                for (let i = 0; i < c.children.length; i++) {
+                    const child = c.children[i];
+                    if (!child.isURDFCollider) {
+                        traverse(c.children[i]);
+                    }
+                }
+            }
+        };
+        traverse(m);
+    };
 
-    const dragControls = new PointerURDFDragControls(
-      this.scene,
-      this.camera,
-      this.renderer.domElement
-    )
-    dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
-      this.setJointValue(joint.name, angle)
-      updateAngle(joint.name, angle)
-    }
-    dragControls.onDragStart = () => {
-      this.orbit.enabled = false
-      this.isDragging = true
-    }
-    dragControls.onDragEnd = () => {
-      this.orbit.enabled = true
-      this.isDragging = false
-    }
-    dragControls.onHover = (joint: URDFMimicJoint) =>
-      this.highlightLinkGeometry(joint, false, highlightMaterial)
-    dragControls.onUnhover = (joint: URDFMimicJoint) =>
-      this.highlightLinkGeometry(joint, true, highlightMaterial)
+    public addTransformControls = (model: any) => {
+        this.transformControl = new TransformControls(this.camera, this.renderer.domElement);
+        this.transformControl.addEventListener('dragging-changed', (event: any) => {
+            this.orbit.enabled = !event.value;
+            this.isDragging = !event.value;
+        });
+        this.transformControl.attach(model);
+        this.scene.add(this.transformControl);
+        this.transformControl.setMode('rotate');
+        return this;
+    };
 
-    this.renderer.domElement.addEventListener(
-      'touchstart',
-      data => dragControls._mouseDown(data.touches[0]),
-      { passive: true }
-    )
-    this.renderer.domElement.addEventListener(
-      'touchmove',
-      data => dragControls._mouseMove(data.touches[0]),
-      { passive: true }
-    )
-    this.renderer.domElement.addEventListener(
-      'touchend',
-      data => dragControls._mouseUp(data.touches[0]),
-      { passive: true }
-    )
-    return this
-  }
+    public addModel = (model: any) => {
+        this.modelGroup = new Group();
+        this.modelGroup.add(model);
+        this.model = model;
+        this.scene.add(this.modelGroup);
+        return this;
+    };
 
-  public toggleFog = () => {
-    this.scene.fog = this.scene.fog ? null : this.fog
-  }
+    public addDragControl = (updateAngle: any) => {
+        const highlightColor = '#FFFFFF';
+        const highlightMaterial = new MeshPhongMaterial({
+            shininess: 10,
+            color: highlightColor,
+            emissive: highlightColor,
+            emissiveIntensity: 0.9
+        });
 
-  private handleRobotShadow = () => {
-    if (this.isLoaded) return
-    const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10)
-    setTimeout(() => clearInterval(intervalId), 1000)
-    this.isLoaded = true
-  }
+        const dragControls = new PointerURDFDragControls(
+            this.scene,
+            this.camera,
+            this.renderer.domElement
+        );
+        dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
+            this.setJointValue(joint.name, angle);
+            updateAngle(joint.name, angle);
+        };
+        dragControls.onDragStart = () => {
+            this.orbit.enabled = false;
+            this.isDragging = true;
+        };
+        dragControls.onDragEnd = () => {
+            this.orbit.enabled = true;
+            this.isDragging = false;
+        };
+        dragControls.onHover = (joint: URDFMimicJoint) =>
+            this.highlightLinkGeometry(joint, false, highlightMaterial);
+        dragControls.onUnhover = (joint: URDFMimicJoint) =>
+            this.highlightLinkGeometry(joint, true, highlightMaterial);
+
+        this.renderer.domElement.addEventListener(
+            'touchstart',
+            data => dragControls._mouseDown(data.touches[0]),
+            { passive: true }
+        );
+        this.renderer.domElement.addEventListener(
+            'touchmove',
+            data => dragControls._mouseMove(data.touches[0]),
+            { passive: true }
+        );
+        this.renderer.domElement.addEventListener(
+            'touchend',
+            data => dragControls._mouseUp(data.touches[0]),
+            { passive: true }
+        );
+        return this;
+    };
+
+    public toggleFog = () => {
+        this.scene.fog = this.scene.fog ? null : this.fog;
+    };
+
+    private handleRobotShadow = () => {
+        if (this.isLoaded) return;
+        const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10);
+        setTimeout(() => clearInterval(intervalId), 1000);
+        this.isLoaded = true;
+    };
 }

app/src/lib/stores/gamepad.ts

@@ -0,0 +1,47 @@
+import { readable, derived } from 'svelte/store'
+
+export type GamepadState = {
+  available: boolean
+  gamepads: Gamepad[]
+}
+
+export const gamepads = readable<GamepadState>({ available: false, gamepads: [] }, set => {
+  const update = () => {
+    const hasGamepadAPI = 'getGamepads' in navigator
+    if (!hasGamepadAPI) {
+      set({ available: false, gamepads: [] })
+      return
+    }
+
+    const gps = navigator.getGamepads?.() ?? []
+    const validGamepads = gps.filter(Boolean) as Gamepad[]
+    set({
+      available: true,
+      gamepads: validGamepads
+    })
+    raf = requestAnimationFrame(update)
+  }
+
+  window.addEventListener('gamepadconnected', update)
+  window.addEventListener('gamepaddisconnected', update)
+  let raf = requestAnimationFrame(update)
+
+  return () => {
+    cancelAnimationFrame(raf)
+    window.removeEventListener('gamepadconnected', update)
+    window.removeEventListener('gamepaddisconnected', update)
+  }
+})
+
+export const gamepad = derived(gamepads, $gamepads =>
+  $gamepads.available && $gamepads.gamepads.length > 0 ? $gamepads.gamepads[0] : null
+)
+
+export const gamepadAxes = derived(gamepad, $gamepad => $gamepad?.axes ?? [0, 0, 0, 0])
+
+export const gamepadButtons = derived(gamepad, $gamepad => $gamepad?.buttons ?? [])
+
+export const hasGamepad = derived(
+  gamepads,
+  $gamepads => $gamepads.available && $gamepads.gamepads.length > 0
+)

app/src/lib/stores/socket.ts

@@ -1,122 +1,132 @@
-import { writable } from 'svelte/store';
+import { writable } from 'svelte/store'
 
-const socketEvents = ['open', 'close', 'error', 'message', 'unresponsive'] as const;
-type SocketEvent = (typeof socketEvents)[number];
+const socketEvents = ['open', 'close', 'error', 'message', 'unresponsive'] as const
+type SocketEvent = (typeof socketEvents)[number]
 
-function createWebSocket() {
-	let listeners = new Map<string, Set<(data?: unknown) => void>>();
-	const { subscribe, set } = writable(false);
-	const reconnectTimeoutTime = 5000;
-	let unresponsiveTimeoutId: number;
-	let reconnectTimeoutId: number;
-	let ws: WebSocket;
-	let socketUrl: string | URL;
-
-	function init(url: string | URL) {
-		socketUrl = url;
-		connect();
-	}
-
-	function disconnect(reason: SocketEvent, event?: Event) {
-		ws.close();
-		set(false);
-		clearTimeout(unresponsiveTimeoutId);
-		clearTimeout(reconnectTimeoutId);
-		listeners.get(reason)?.forEach((listener) => listener(event));
-		reconnectTimeoutId = setTimeout(connect, reconnectTimeoutTime);
-	}
-
-	function connect() {
-		ws = new WebSocket(socketUrl);
-		ws.onopen = (ev) => {
-			set(true);
-			clearTimeout(reconnectTimeoutId);
-			listeners.get('open')?.forEach((listener) => listener(ev));
-			for (const event of listeners.keys()) {
-				if (socketEvents.includes(event as SocketEvent)) continue;
-				subscribeToEvent(event);
-			}
-		};
-		ws.onmessage = (message) => {
-			resetUnresponsiveCheck();
-			let data = message.data;
-			if (data instanceof ArrayBuffer) {
-				listeners.get('binary')?.forEach((listener) => listener(data));
-				return;
-			}
-			data = data.substring(1);
-
-			if (!data) return;
-
-			let event = data.substring(data.indexOf('/') + 1, data.indexOf('['));
-			let payload = data.substring(data.indexOf('[') + 1, data.lastIndexOf(']'));
-
-			try {
-				payload = JSON.parse(payload);
-			} catch (error) {}
-			if (event) listeners.get(event)?.forEach((listener) => listener(payload));
-		};
-		ws.onerror = (ev) => disconnect('error', ev);
-		ws.onclose = (ev) => disconnect('close', ev);
-	}
-
-	function unsubscribe(event: string, listener?: (data: any) => void) {
-		let eventListeners = listeners.get(event);
-		if (!eventListeners) return;
-
-		if (!eventListeners.size) {
-			unsubscribeToEvent(event);
-		}
-		if (listener) {
-			eventListeners?.delete(listener);
-		} else {
-			listeners.delete(event);
-		}
-	}
-
-	function resetUnresponsiveCheck() {
-		clearTimeout(unresponsiveTimeoutId);
-		unresponsiveTimeoutId = setTimeout(() => disconnect('unresponsive'), reconnectTimeoutTime);
-	}
-
-	function sendEvent(event: string, data: unknown) {
-		if (!ws || ws.readyState !== WebSocket.OPEN) return;
-		ws.send(`2/${event}[${JSON.stringify(data)}]`);
-	}
-
-	function unsubscribeToEvent(event: string) {
-		if (!ws || ws.readyState !== WebSocket.OPEN) return;
-		ws.send('1/' + event);
-	}
-
-	function subscribeToEvent(event: string) {
-		if (!ws || ws.readyState !== WebSocket.OPEN) return;
-		ws.send('0/' + event);
-	}
-
-	return {
-		subscribe,
-		sendEvent,
-		init,
-		on: <T>(event: string, listener: (data: T) => void): (() => void) => {
-			let eventListeners = listeners.get(event);
-			if (!eventListeners) {
-				if (!socketEvents.includes(event as SocketEvent)) {
-					subscribeToEvent(event);
-				}
-				eventListeners = new Set();
-				listeners.set(event, eventListeners);
-			}
-			eventListeners.add(listener as (data: any) => void);
-
-			return () => {
-				unsubscribe(event, listener);
-			};
-		},
-		off: (event: string, listener?: (data: any) => void) => {
-			unsubscribe(event, listener);
-		}
-	};
+export enum Topics {
+  imu = 0,
+  mode = 1,
+  command = 2,
+  servo = 3,
+  input = 4,
+  angles = 5,
+  position = 6
 }
 
-export const socket = createWebSocket();
+function createWebSocket() {
+  let listeners = new Map<string | Topics, Set<(data?: unknown) => void>>()
+  const { subscribe, set } = writable(false)
+  const reconnectTimeoutTime = 5000
+  let unresponsiveTimeoutId: number
+  let reconnectTimeoutId: number
+  let ws: WebSocket
+  let socketUrl: string | URL
+
+  function init(url: string | URL) {
+    socketUrl = url
+    connect()
+  }
+
+  function disconnect(reason: SocketEvent, event?: Event) {
+    ws.close()
+    set(false)
+    clearTimeout(unresponsiveTimeoutId)
+    clearTimeout(reconnectTimeoutId)
+    listeners.get(reason)?.forEach(listener => listener(event))
+    reconnectTimeoutId = setTimeout(connect, reconnectTimeoutTime)
+  }
+
+  function connect() {
+    ws = new WebSocket(socketUrl)
+    ws.onopen = ev => {
+      set(true)
+      clearTimeout(reconnectTimeoutId)
+      listeners.get('open')?.forEach(listener => listener(ev))
+      for (const event of listeners.keys()) {
+        if (socketEvents.includes(event as SocketEvent)) continue
+        subscribeToEvent(event as unknown as Topics)
+      }
+    }
+    ws.onmessage = message => {
+      resetUnresponsiveCheck()
+      let data = message.data
+      if (data instanceof ArrayBuffer) {
+        listeners.get('binary')?.forEach(listener => listener(data))
+        return
+      }
+      data = data.substring(1)
+
+      if (!data) return
+
+      let event = data.substring(data.indexOf('/') + 1, data.indexOf('['))
+      let payload = data.substring(data.indexOf('[') + 1, data.lastIndexOf(']'))
+
+      try {
+        payload = JSON.parse(payload)
+      } catch (error) {}
+      if (event) listeners.get(event)?.forEach(listener => listener(payload))
+    }
+    ws.onerror = ev => disconnect('error', ev)
+    ws.onclose = ev => disconnect('close', ev)
+  }
+
+  function unsubscribe(event: Topics, listener?: (data: any) => void) {
+    let eventListeners = listeners.get(event)
+    if (!eventListeners) return
+
+    if (!eventListeners.size) {
+      unsubscribeToEvent(event)
+    }
+    if (listener) {
+      eventListeners?.delete(listener)
+    } else {
+      listeners.delete(event)
+    }
+  }
+
+  function resetUnresponsiveCheck() {
+    clearTimeout(unresponsiveTimeoutId)
+    unresponsiveTimeoutId = setTimeout(() => disconnect('unresponsive'), reconnectTimeoutTime)
+  }
+
+  function sendEvent(event: Topics, data: unknown) {
+    if (!ws || ws.readyState !== WebSocket.OPEN) return
+    ws.send(JSON.stringify([2, event, data]))
+  }
+
+  function unsubscribeToEvent(event: Topics) {
+    if (!ws || ws.readyState !== WebSocket.OPEN) return
+    ws.send(`[1,${event}]`)
+  }
+
+  function subscribeToEvent(event: Topics) {
+    if (!ws || ws.readyState !== WebSocket.OPEN) return
+    ws.send(`[0,${event}]`)
+  }
+
+  return {
+    subscribe,
+    sendEvent,
+    init,
+    on: <T>(event: Topics | SocketEvent, listener: (data: T) => void): (() => void) => {
+      let eventListeners = listeners.get(event)
+      if (!eventListeners) {
+        if (!socketEvents.includes(event)) {
+          subscribeToEvent(event)
+        }
+        eventListeners = new Set()
+        listeners.set(event, eventListeners)
+      }
+      eventListeners.add(listener as (data: any) => void)
+
+      return () => {
+        unsubscribe(event, listener)
+      }
+    },
+    off: (event: Topics, listener?: (data: any) => void) => {
+      unsubscribe(event, listener)
+    }
+  }
+}
+
+export const socket = createWebSocket()

app/src/lib/types/models.ts

@@ -1,178 +1,221 @@
-export type vector = { x: number; y: number };
+export type vector = { x: number; y: number }
 
 export interface ControllerInput {
-    left: vector;
-    right: vector;
-    height: number;
-    speed: number;
-    s1: number;
+  left: vector
+  right: vector
+  height: number
+  speed: number
+  s1: number
 }
 
 export type GithubRelease = {
-    message: string;
-    tag_name: string;
-    assets: Array<{
-        name: string;
-        browser_download_url: string;
-    }>;
-};
+  message: string
+  tag_name: string
+  assets: Array<{
+    name: string
+    browser_download_url: string
+  }>
+}
 
-export type angles = number[] | Int16Array;
+export type angles = number[] | Int16Array
 
 export type WifiStatus = {
-    status: number;
-    local_ip: string;
-    mac_address: string;
-    rssi: number;
-    ssid: string;
-    bssid: string;
-    channel: number;
-    subnet_mask: string;
-    gateway_ip: string;
-    dns_ip_1: string;
-    dns_ip_2?: string;
-};
+  status: number
+  local_ip: string
+  mac_address: string
+  rssi: number
+  ssid: string
+  bssid: string
+  channel: number
+  subnet_mask: string
+  gateway_ip: string
+  dns_ip_1: string
+  dns_ip_2?: string
+}
 
 export type WifiSettings = {
-    hostname: string;
-    priority_RSSI: boolean;
-    wifi_networks: KnownNetworkItem[];
-};
+  hostname: string
+  priority_RSSI: boolean
+  wifi_networks: KnownNetworkItem[]
+}
 
 export type NetworkList = {
-    networks: NetworkItem[];
-};
+  networks: NetworkItem[]
+}
 
 export type KnownNetworkItem = {
-    ssid: string;
-    password: string;
-    static_ip_config: boolean;
-    local_ip?: string;
-    subnet_mask?: string;
-    gateway_ip?: string;
-    dns_ip_1?: string;
-    dns_ip_2?: string;
-};
+  ssid: string
+  password: string
+  static_ip_config: boolean
+  local_ip?: string
+  subnet_mask?: string
+  gateway_ip?: string
+  dns_ip_1?: string
+  dns_ip_2?: string
+}
 
 export type NetworkItem = {
-    rssi: number;
-    ssid: string;
-    bssid: string;
-    channel: number;
-    encryption_type: number;
-};
+  rssi: number
+  ssid: string
+  bssid: string
+  channel: number
+  encryption_type: number
+}
 
 export type ApStatus = {
-    status: number;
-    ip_address: string;
-    mac_address: string;
-    station_num: number;
-};
+  status: number
+  ip_address: string
+  mac_address: string
+  station_num: number
+}
 
 export type ApSettings = {
-    provision_mode: number;
-    ssid: string;
-    password: string;
-    channel: number;
-    ssid_hidden: boolean;
-    max_clients: number;
-    local_ip: string;
-    gateway_ip: string;
-    subnet_mask: string;
-};
+  provision_mode: number
+  ssid: string
+  password: string
+  channel: number
+  ssid_hidden: boolean
+  max_clients: number
+  local_ip: string
+  gateway_ip: string
+  subnet_mask: string
+}
 
 export type DownloadOTA = {
-    status: string;
-    progress: number;
-    error: string;
-};
+  status: string
+  progress: number
+  error: string
+}
 
 export type Analytics = {
-    max_alloc_heap: number;
-    psram_size: number;
-    free_psram: number;
-    free_heap: number;
-    total_heap: number;
-    min_free_heap: number;
-    core_temp: number;
-    fs_total: number;
-    fs_used: number;
-    uptime: number;
-    cpu0_usage: number;
-    cpu1_usage: number;
-    cpu_usage: number;
-};
+  max_alloc_heap: number
+  psram_size: number
+  free_psram: number
+  free_heap: number
+  total_heap: number
+  min_free_heap: number
+  core_temp: number
+  fs_total: number
+  fs_used: number
+  uptime: number
+  cpu0_usage: number
+  cpu1_usage: number
+  cpu_usage: number
+}
 
 export type Rssi = {
-    rssi: number;
-    ssid: string;
-};
+  rssi: number
+  ssid: string
+}
 
 export type StaticSystemInformation = {
-    esp_platform: string;
-    firmware_version: string;
-    cpu_freq_mhz: number;
-    cpu_type: string;
-    cpu_rev: number;
-    cpu_cores: number;
-    sketch_size: number;
-    free_sketch_space: number;
-    sdk_version: string;
-    arduino_version: string;
-    flash_chip_size: number;
-    flash_chip_speed: number;
-    cpu_reset_reason: string;
-};
+  esp_platform: string
+  firmware_version: string
+  cpu_freq_mhz: number
+  cpu_type: string
+  cpu_rev: number
+  cpu_cores: number
+  sketch_size: number
+  free_sketch_space: number
+  sdk_version: string
+  arduino_version: string
+  flash_chip_size: number
+  flash_chip_speed: number
+  cpu_reset_reason: string
+}
 
-export type SystemInformation = Analytics & StaticSystemInformation;
+export type SystemInformation = Analytics & StaticSystemInformation
 
 export type IMU = {
-    x: number;
-    y: number;
-    z: number;
-    heading: number;
-    altitude: number;
-    bmp_temp: number;
-    pressure: number;
-};
+  x: number
+  y: number
+  z: number
+  heading: number
+  altitude: number
+  bmp_temp: number
+  pressure: number
+}
 
 export interface I2CDevice {
-    address: number;
-    part_number: string;
-    name: string;
+  address: number
+  part_number: string
+  name: string
+}
+
+export type PinConfig = {
+  pin: number
+  mode: string
+  type: string
+  role: string
+}
+
+export type PeripheralsConfiguration = {
+  sda: number
+  scl: number
+  frequency: number
+  pins: PinConfig[]
 }
 
 export type CameraSettings = {
-    framesize: number;
-    quality: number;
-    brightness: number;
-    contrast: number;
-    saturation: number;
-    sharpness: number;
-    denoise: number;
-    special_effect: number;
-    wb_mode: number;
-    vflip: boolean;
-    hmirror: boolean;
-};
+  framesize: number
+  quality: number
+  brightness: number
+  contrast: number
+  saturation: number
+  sharpness: number
+  denoise: number
+  special_effect: number
+  wb_mode: number
+  vflip: boolean
+  hmirror: boolean
+}
 
-export type File = number;
+export type File = number
 
 export interface Directory {
-    [key: string]: File | Directory;
+  [key: string]: File | Directory
 }
 
 export type Servo = {
-    name: string;
-    channel: number;
-    inverted: boolean;
-    angle: number;
-    center_angle: number;
-};
+  name: string
+  channel: number
+  inverted: boolean
+  angle: number
+  center_angle: number
+}
 
 export type ServoConfiguration = {
-    is_active: boolean;
-    servo_pwm_frequency: number;
-    servo_oscillator_frequency: number;
-    servos: Servo[];
-};
+  is_active: boolean
+  servo_pwm_frequency: number
+  servo_oscillator_frequency: number
+  servos: Servo[]
+}
+
+export interface MDNSServiceQuery {
+  services: MDNSServiceItem[]
+}
+
+export interface MDNSServiceItem {
+  ip: string
+  port: number
+  name: string
+}
+
+export interface MDNSService {
+  service: string
+  protocol: string
+  port: number
+}
+
+export interface MDNSTxtRecord {
+  key: string
+  value: string
+}
+
+export interface MDNSStatus {
+  started: boolean
+  hostname: string
+  instance: string
+  services: MDNSService[]
+  global_txt_records: MDNSTxtRecord[]
+}

app/src/lib/utilities/model-utilities.ts

@@ -1,89 +1,92 @@
-import { Color, LoaderUtils, Vector3 } from 'three';
-import URDFLoader, { type URDFRobot } from 'urdf-loader';
-import { XacroLoader } from 'xacro-parser';
-import { Result } from '$lib/utilities';
-import { jointNames, model } from '$lib/stores';
-import uzip from 'uzip';
-import { fileService } from '$lib/services';
+import { Color, LoaderUtils, Vector3 } from 'three'
+import URDFLoader, { type URDFRobot } from 'urdf-loader'
+import { XacroLoader } from 'xacro-parser'
+import { Result } from '$lib/utilities'
+import { jointNames, model } from '$lib/stores'
+import uzip from 'uzip'
+import { fileService } from '$lib/services'
 
-let model_xml: XMLDocument;
+let model_xml: XMLDocument
 
 export const populateModelCache = async () => {
-	await cacheModelFiles();
-	const modelRes = await loadModelAsync('/spot_micro.urdf.xacro');
-	if (modelRes.isOk()) {
-		const [urdf, JOINT_NAME] = modelRes.inner;
-		jointNames.set(JOINT_NAME);
-		model.set(urdf);
-	} else {
-		console.error(modelRes.inner, { exception: modelRes.exception });
-	}
-};
+  await cacheModelFiles()
+  const modelRes = await loadModel('/yertle.URDF')
+  if (modelRes.isOk()) {
+    const [urdf, JOINT_NAME] = modelRes.inner
+    jointNames.set(JOINT_NAME)
+    model.set(urdf)
+  } else {
+    console.error(modelRes.inner, { exception: modelRes.exception })
+  }
+}
 
 export const cacheModelFiles = async () => {
-	let data = await fetch('/stl.zip');
+  const data = await fetch('/URDF.zip')
 
-	var files = uzip.parse(await data.arrayBuffer());
+  const files = uzip.parse(await data.arrayBuffer())
 
-	for (const [path, data] of Object.entries(files) as [path: string, data: Uint8Array][]) {
-		const url = new URL(path, window.location.href);
-		fileService.saveFile(url.toString(), data);
-	}
-};
+  for (const [path, data] of Object.entries(files) as [path: string, data: Uint8Array][]) {
+    const url = new URL(path, window.location.href)
+    fileService?.saveFile(url.toString(), data)
+  }
+}
 
-export const loadModelAsync = async (
-	url: string
-): Promise<Result<[URDFRobot, string[]], string>> => {
-	return new Promise((resolve, reject) => {
-		const xacroLoader = new XacroLoader();
-		const urdfLoader = new URDFLoader();
-		urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
+export const loadModel = async (url: string): Promise<Result<[URDFRobot, string[]], string>> => {
+  const urdfLoader = new URDFLoader()
+  urdfLoader.workingPath = LoaderUtils.extractUrlBase(url)
 
-		xacroLoader.load(
-			url,
-			async (xml) => {
-				model_xml = xml;
-				try {
-					const model = urdfLoader.parse(xml);
-					model.rotation.x = -Math.PI / 2;
-					model.rotation.z = Math.PI / 2;
-					model.traverse((c) => (c.castShadow = true));
-					model.updateMatrixWorld(true);
-					model.scale.setScalar(10);
-					const joints = Object.entries(model.joints)
-						.filter((joint) => joint[1].jointType !== 'fixed')
-						.map((joint) => joint[0]);
+  let xml = url.endsWith('.xacro') ? await loadXacro(url) : await fetch(url).then(res => res.text())
 
-					resolve(Result.ok([model, joints]));
-				} catch (error) {
-					resolve(Result.err('Failed to load model', error));
-				}
-			},
-			(error) => resolve(Result.err('Failed to load model', error))
-		);
-	});
-};
+  if (typeof xml === 'string') {
+    xml = new window.DOMParser().parseFromString(xml, 'text/xml')
+  }
 
-export const toeWorldPositions = (robot: URDFRobot) => {
-	const toe_positions: Vector3[] = [];
-	robot.traverse((child) => {
-		if (child.name.includes('toe') && !child.name.includes('_link')) {
-			const worldPosition = new Vector3();
-			child.getWorldPosition(worldPosition);
-			toe_positions.push(worldPosition);
-		}
-	});
-	return toe_positions;
-};
+  return new Promise(resolve => {
+    model_xml = xml
+    try {
+      const model = urdfLoader.parse(xml)
+      setupRobot(model)
+      const joints = Object.entries(model.joints)
+        .filter(joint => joint[1].jointType !== 'fixed')
+        .map(joint => joint[0])
 
-export const footColor = () => {
-	const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element;
-	const colorAttrStr = colorElem.getAttribute('rgba') as string;
-	const colorStr = colorAttrStr
-		.split(' ')
-		.slice(0, 3)
-		.map((val) => Math.floor(+val * 255))
-		.join(', ');
+      resolve(Result.ok([model, joints]))
+    } catch (error) {
+      resolve(Result.err('Failed to load model', error))
+    }
+  })
+}
 
-	return new Color(`rgb(${colorStr})`);
-};
+const loadXacro = async (url: string): Promise<XMLDocument> =>
+  new Promise((resolve, reject) => {
+    new XacroLoader().load(url, resolve, reject)
+  })
+
+function setupRobot(robot: URDFRobot) {
+  robot.rotation.x = -Math.PI / 2
+  robot.rotation.z = Math.PI / 2
+  robot.scale.setScalar(10)
+  robot.traverse(c => (c.castShadow = true))
+  robot.updateMatrixWorld(true)
+}
+
+export function getToeWorldPositions(robot: URDFRobot): Vector3[] {
+  const toes: Vector3[] = []
+  robot.traverse(c => {
+    if (c.name.includes('toe') && !c.name.includes('_link'))
+      toes.push(c.getWorldPosition(new Vector3()))
+  })
+  return toes
+}
+
+export const extractFootColor = () => {
+  const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element
+  const colorAttrStr = colorElem.getAttribute('rgba') as string
+  const colorStr = colorAttrStr
+    .split(' ')
+    .slice(0, 3)
+    .map(val => Math.floor(+val * 255))
+    .join(', ')
+
+  return new Color(`rgb(${colorStr})`)
+}

app/src/lib/utilities/string-utilities.ts

@@ -1,36 +1,47 @@
 export const humanFileSize = (size: number): string => {
-	const units = ['B', 'kB', 'MB', 'GB', 'TB'];
-	var i = size == 0 ? 0 : Math.floor(Math.log(size) / Math.log(1024));
-	return Number((size / Math.pow(1024, i)).toFixed(2)) * 1 + units[i];
-};
+  const units = ['B', 'kB', 'MB', 'GB', 'TB']
+  const i = size == 0 ? 0 : Math.floor(Math.log(size) / Math.log(1024))
+  return Number((size / Math.pow(1024, i)).toFixed(2)) * 1 + units[i]
+}
 
 export const capitalize = (str: string): string => {
-	return str.charAt(0).toUpperCase() + str.slice(1).toLowerCase();
-};
+  return str.charAt(0).toUpperCase() + str.slice(1).toLowerCase()
+}
 
 export const convertSeconds = (seconds: number) => {
-	// Calculate the number of seconds, minutes, hours, and days
-	let minutes = Math.floor(seconds / 60);
-	let hours = Math.floor(minutes / 60);
-	let days = Math.floor(hours / 24);
+  // Calculate the number of seconds, minutes, hours, and days
+  let minutes = Math.floor(seconds / 60)
+  let hours = Math.floor(minutes / 60)
+  const days = Math.floor(hours / 24)
 
-	// Calculate the remaining hours, minutes, and seconds
-	hours = hours % 24;
-	minutes = minutes % 60;
-	seconds = seconds % 60;
+  // Calculate the remaining hours, minutes, and seconds
+  hours = hours % 24
+  minutes = minutes % 60
+  seconds = seconds % 60
 
-	// Create the formatted string
-	let result = '';
-	if (days > 0) {
-		result += days + ' day' + (days > 1 ? 's' : '') + ' ';
-	}
-	if (hours > 0) {
-		result += hours + ' hour' + (hours > 1 ? 's' : '') + ' ';
-	}
-	if (minutes > 0) {
-		result += minutes + ' minute' + (minutes > 1 ? 's' : '') + ' ';
-	}
-	result += seconds + ' second' + (seconds > 1 ? 's' : '');
+  // Create the formatted string
+  let result = ''
+  if (days > 0) {
+    result += days + ' day' + (days > 1 ? 's' : '') + ' '
+  }
+  if (hours > 0) {
+    result += hours + ' hour' + (hours > 1 ? 's' : '') + ' '
+  }
+  if (minutes > 0) {
+    result += minutes + ' minute' + (minutes > 1 ? 's' : '') + ' '
+  }
+  result += seconds + ' second' + (seconds > 1 ? 's' : '')
 
-	return result;
-};
+  return result
+}
+
+export const compareIp = (ip1: string, ip2: string) => {
+  const ip1Parts = ip1.split('.').map(Number)
+  const ip2Parts = ip2.split('.').map(Number)
+  for (let i = 0; i < 4; i++) {
+    if (ip1Parts[i] !== ip2Parts[i]) {
+      return ip1Parts[i] > ip2Parts[i] ? 1 : -1
+    }
+  }
+  return 0
+}

app/src/routes/+layout.svelte

@@ -1,125 +1,125 @@
 <script lang="ts">
-  import { onDestroy, onMount } from 'svelte'
-  import { page } from '$app/state'
-  import { Modals, modals } from 'svelte-modals'
-  import Toast from '$lib/components/toasts/Toast.svelte'
-  import { notifications } from '$lib/components/toasts/notifications'
-  import { fade } from 'svelte/transition'
-  import '../app.css'
-  import Menu from '../lib/components/menu/Menu.svelte'
-  import Statusbar from '../lib/components/statusbar/statusbar.svelte'
-  import {
-    telemetry,
-    analytics,
-    ModesEnum,
-    kinematicData,
-    mode,
-    outControllerData,
-    servoAngles,
-    servoAnglesOut,
-    socket,
-    location,
-    useFeatureFlags
-  } from '$lib/stores'
-  import type { Analytics, DownloadOTA } from '$lib/types/models'
-  interface Props {
-    children?: import('svelte').Snippet
-  }
+    import { onDestroy, onMount } from 'svelte';
+    import { page } from '$app/state';
+    import { Modals, modals } from 'svelte-modals';
+    import Toast from '$lib/components/toasts/Toast.svelte';
+    import { notifications } from '$lib/components/toasts/notifications';
+    import { fade } from 'svelte/transition';
+    import '../app.css';
+    import Menu from '../lib/components/menu/Menu.svelte';
+    import Statusbar from '../lib/components/statusbar/statusbar.svelte';
+    import {
+        telemetry,
+        analytics,
+        ModesEnum,
+        kinematicData,
+        mode,
+        outControllerData,
+        servoAngles,
+        servoAnglesOut,
+        socket,
+        location,
+        useFeatureFlags
+    } from '$lib/stores';
+    import type { Analytics, DownloadOTA } from '$lib/types/models';
+    interface Props {
+        children?: import('svelte').Snippet;
+    }
 
-  let { children }: Props = $props()
+    let { children }: Props = $props();
 
-  const features = useFeatureFlags()
+    const features = useFeatureFlags();
 
-  onMount(async () => {
-    const ws = $location ? $location : window.location.host
-    socket.init(`ws://${ws}/api/ws/events`)
+    onMount(async () => {
+        const ws = $location ? $location : window.location.host;
+        socket.init(`ws://${ws}/api/ws/events`);
 
-    addEventListeners()
+        addEventListeners();
 
-    outControllerData.subscribe(data => socket.sendEvent('input', { data }))
-    mode.subscribe(data => socket.sendEvent('mode', { data }))
-    servoAnglesOut.subscribe(data => socket.sendEvent('angles', { data }))
-    kinematicData.subscribe(data => socket.sendEvent('position', { data }))
-  })
+        outControllerData.subscribe(data => socket.sendEvent('input', { data }));
+        mode.subscribe(data => socket.sendEvent('mode', { data }));
+        servoAnglesOut.subscribe(data => socket.sendEvent('angles', { data }));
+        kinematicData.subscribe(data => socket.sendEvent('position', { data }));
+    });
 
-  onDestroy(() => {
-    removeEventListeners()
-  })
+    onDestroy(() => {
+        removeEventListeners();
+    });
 
-  const addEventListeners = () => {
-    socket.on('open', handleOpen)
-    socket.on('close', handleClose)
-    socket.on('error', handleError)
-    socket.on('rssi', handleNetworkStatus)
-    socket.on('mode', (data: ModesEnum) => mode.set(data))
-    socket.on('analytics', handleAnalytics)
-    socket.on('angles', (angles: number[]) => {
-      if (angles.length) servoAngles.set(angles)
-    })
-    features.subscribe(data => {
-      if (data?.download_firmware) socket.on('otastatus', handleOAT)
-      if (data?.sonar) socket.on('sonar', data => console.log(data))
-    })
-  }
+    const addEventListeners = () => {
+        socket.on('open', handleOpen);
+        socket.on('close', handleClose);
+        socket.on('error', handleError);
+        socket.on('rssi', handleNetworkStatus);
+        socket.on('mode', (data: ModesEnum) => mode.set(data));
+        socket.on('analytics', handleAnalytics);
+        socket.on('angles', (angles: number[]) => {
+            if (angles.length) servoAngles.set(angles);
+        });
+        features.subscribe(data => {
+            if (data?.download_firmware) socket.on('otastatus', handleOAT);
+            if (data?.sonar) socket.on('sonar', data => console.log(data));
+        });
+    };
 
-  const removeEventListeners = () => {
-    socket.off('analytics', handleAnalytics)
-    socket.off('open', handleOpen)
-    socket.off('close', handleClose)
-    socket.off('rssi', handleNetworkStatus)
-    socket.off('otastatus', handleOAT)
-  }
+    const removeEventListeners = () => {
+        socket.off('analytics', handleAnalytics);
+        socket.off('open', handleOpen);
+        socket.off('close', handleClose);
+        socket.off('rssi', handleNetworkStatus);
+        socket.off('otastatus', handleOAT);
+    };
 
-  const handleOpen = () => {
-    notifications.success('Connection to device established', 5000)
-  }
+    const handleOpen = () => {
+        notifications.success('Connection to device established', 5000);
+    };
 
-  const handleClose = () => {
-    notifications.error('Connection to device lost', 5000)
-    telemetry.setRSSI(0)
-  }
+    const handleClose = () => {
+        notifications.error('Connection to device lost', 5000);
+        telemetry.setRSSI(0);
+    };
 
-  const handleError = (data: any) => console.error(data)
+    const handleError = (data: any) => console.error(data);
 
-  const handleAnalytics = (data: Analytics) => analytics.addData(data)
+    const handleAnalytics = (data: Analytics) => analytics.addData(data);
 
-  const handleNetworkStatus = (data: number) => telemetry.setRSSI(data)
+    const handleNetworkStatus = (data: number) => telemetry.setRSSI(data);
 
-  const handleOAT = (data: DownloadOTA) => telemetry.setDownloadOTA(data)
+    const handleOAT = (data: DownloadOTA) => telemetry.setDownloadOTA(data);
 
-  let menuOpen = $state(false)
+    let menuOpen = $state(false);
 </script>
 
 <svelte:head>
-  <title>{page.data.title}</title>
+    <title>{page.data.title}</title>
 </svelte:head>
 
-<div class="drawer h-screen">
-  <input id="main-menu" type="checkbox" class="drawer-toggle" bind:checked={menuOpen} />
-  <div class="drawer-content flex flex-col">
-    <!-- Status bar content here -->
-    <Statusbar />
+<div class="drawer">
+    <input id="main-menu" type="checkbox" class="drawer-toggle" bind:checked={menuOpen} />
+    <div class="drawer-content flex flex-col">
+        <!-- Status bar content here -->
+        <Statusbar />
 
-    <!-- Main page content here -->
-    {@render children?.()}
-  </div>
-  <!-- Side Navigation -->
-  <div class="drawer-side z-30 shadow-lg">
-    <label for="main-menu" class="drawer-overlay"></label>
-    <Menu menuClicked={() => (menuOpen = false)} />
-  </div>
+        <!-- Main page content here -->
+        {@render children?.()}
+    </div>
+    <!-- Side Navigation -->
+    <div class="drawer-side z-30 shadow-lg">
+        <label for="main-menu" class="drawer-overlay"></label>
+        <Menu menuClicked={() => (menuOpen = false)} />
+    </div>
 </div>
 
 <Modals>
-  <!-- svelte-ignore a11y_click_events_have_key_events -->
-  <!-- svelte-ignore a11y_no_static_element_interactions -->
-  {#snippet backdrop()}
-    <div
-      class="fixed inset-0 z-40 max-h-full max-w-full bg-black/20 backdrop-blur-sm"
-      transition:fade
-      onclick={modals.closeAll}>
-    </div>
-  {/snippet}
+    <!-- svelte-ignore a11y_click_events_have_key_events -->
+    <!-- svelte-ignore a11y_no_static_element_interactions -->
+    {#snippet backdrop()}
+        <div
+            class="fixed inset-0 z-40 max-h-full max-w-full bg-black/20 backdrop-blur-sm"
+            transition:fade
+            onclick={modals.closeAll}
+        ></div>
+    {/snippet}
 </Modals>
 
 <Toast />

app/src/routes/+page.svelte

@@ -13,17 +13,15 @@
   })
 </script>
 
-<div class="w-full h-full flex justify-center items-center">
-  <div class="h-full flex flex-col">
-    <div class="grow-3 w-80 relative">
-      <Visualization sky={false} orbit panel={false} ground={false} zoom={8} />
-      <div class="absolute bottom-0 w-full h-40 bg-gradient-to-t from-base-100 to-transparent">
-      </div>
+<div class="hero bg-base-100 h-screen">
+  <div class="card md:card-side bg-base-200 shadow-2xl flex justify-center items-center">
+    <div class="w-64 h-64">
+      <Visualization sky={false} orbit panel={false} ground={false} />
     </div>
-    <div class="grow-3 flex justify-center">
-      <a class="btn btn-primary rounded-full" href={$socket ? '/controller' : '/connection'}>
-        Add Robot Dog
-      </a>
+    <div class="card-body w-80">
+      <h2 class="card-title text-center text-2xl">Begin you journey</h2>
+      <p class="py-6 text-center"></p>
+      <a class="btn btn-primary" href={$socket ? '/controller' : '/connection'}> Add Robot Dog </a>
     </div>
   </div>
 </div>

app/src/routes/controller/Controls.svelte

@@ -5,6 +5,8 @@
   import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores'
   import type { vector } from '$lib/types/models'
   import { VerticalSlider } from '$lib/components/input'
+  import { gamepadAxes, gamepadButtons, hasGamepad } from '$lib/stores/gamepad'
+  import { notifications } from '$lib/components/toasts/notifications'
 
   let throttle = new throttler()
   let left: nipplejs.JoystickManager
@@ -13,6 +15,23 @@
   let throttle_timing = 40
   let data = new Array(8)
 
+  $effect(() => {
+    if ($hasGamepad) {
+      notifications.success('🎮 Gamepad connected', 3000)
+    }
+  })
+
+  $effect(() => {
+    handleJoyMove('left', { x: $gamepadAxes[0], y: -$gamepadAxes[1] })
+    handleJoyMove('right', { x: $gamepadAxes[2], y: $gamepadAxes[3] })
+  })
+
+  // TODO React to button press
+  //   $effect(() => {
+  //     if ($gamepadButtons.length === 0) return
+  //
+  //   })
+
   onMount(() => {
     left = nipplejs.create({
       zone: document.getElementById('left') as HTMLElement,
@@ -70,7 +89,7 @@
   }
 
   const handleRange = (event: Event, key: 'speed' | 'height' | 's1') => {
-    const value: number = event.target?.value
+    const value: number = Number((event.target as HTMLInputElement).value)
 
     input.update(inputData => {
       inputData[key] = value
@@ -127,7 +146,7 @@
               type="range"
               name="s1"
               min="0"
-              max="100"
+              max="25"
               oninput={e => handleRange(e, 's1')}
               class="range range-sm range-primary" />
           </div>
@@ -137,7 +156,7 @@
               type="range"
               name="speed"
               min="0"
-              max="100"
+              max="25"
               oninput={e => handleRange(e, 'speed')}
               class="range range-sm range-primary" />
           </div>

app/src/routes/peripherals/i2c/+page.svelte

@@ -1,7 +1,7 @@
 <script lang="ts">
-	import I2C from './i2c.svelte';
+  import I2C from './i2c.svelte'
 </script>
 
 <div class="mx-0 my-1 flex flex-col space-y-4 sm:mx-8 sm:my-8">
-	<I2C />
+  <I2C />
 </div>

app/src/routes/peripherals/i2c/+page.ts

@@ -1,7 +1,7 @@
-import type { PageLoad } from './$types';
+import type { PageLoad } from './$types'
 
 export const load = (async () => {
-	return {
-		title: 'I2C'
-	};
-}) satisfies PageLoad;
+  return {
+    title: 'I2C'
+  }
+}) satisfies PageLoad

app/src/routes/peripherals/i2c/i2c.svelte

@@ -1,57 +1,78 @@
 <script lang="ts">
-    import SettingsCard from '$lib/components/SettingsCard.svelte';
-    import { onMount } from 'svelte';
-    import { socket } from '$lib/stores';
-    import type { I2CDevice } from '$lib/types/models';
-    import { Connection } from '$lib/components/icons';
+  import SettingsCard from '$lib/components/SettingsCard.svelte'
+  import { onMount } from 'svelte'
+  import { socket } from '$lib/stores'
+  import type { I2CDevice } from '$lib/types/models'
+  import { Connection } from '$lib/components/icons'
+  import I2CSetting from './i2cSetting.svelte'
 
-    const i2cDevices = [
-        { address: 30, part_number: 'HMC5883', name: '3-Axis Digital Compass/Magnetometer IC' },
-        { address: 64, part_number: 'PCA9685', name: '16-channel PWM driver default address' },
-        { address: 72, part_number: 'ADS1115', name: '4-channel 16-bit ADC' },
-        {
-            address: 104,
-            part_number: 'MPU6050',
-            name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices'
-        },
-        { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' }
-    ];
+  const i2cDevices = [
+    { address: 30, part_number: 'HMC5883', name: '3-Axis Digital Compass/Magnetometer IC' },
+    { address: 41, part_number: 'BNO055', name: '9-Axis Absolute Orientation Sensor' },
+    { address: 64, part_number: 'PCA9685', name: '16-channel PWM driver default address' },
+    { address: 72, part_number: 'ADS1115', name: '4-channel 16-bit ADC' },
+    {
+      address: 104,
+      part_number: 'MPU6050',
+      name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices'
+    },
+    { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' }
+  ]
 
-    let active_devices: I2CDevice[] = $state([]);
+  let active_devices: I2CDevice[] = $state([])
 
-    onMount(() => {
-        socket.on('i2cScan', handleScan);
-        socket.sendEvent('i2cScan', '');
-        return () => socket.off('i2cScan', handleScan);
-    });
+  let isLoading = $state(false)
 
-    const handleScan = (data: any) => {
-        active_devices = data.addresses.map(
-            (address: number) =>
-                i2cDevices.find(device => device.address === address) || {
-                    address,
-                    part_number: 'Unknown',
-                    name: 'Unknown'
-                }
-        );
-    };
+  onMount(() => {
+    socket.on('i2cScan', handleScan)
+    triggerScan()
+    return () => socket.off('i2cScan', handleScan)
+  })
+
+  const handleScan = (data: any) => {
+    active_devices = data.addresses.map(
+      (address: number) =>
+        i2cDevices.find(device => device.address === address) || {
+          address,
+          part_number: 'Unknown',
+          name: 'Unknown'
+        }
+    )
+    isLoading = false
+  }
+
+  const triggerScan = () => {
+    isLoading = true
+    socket.sendEvent('i2cScan', '')
+  }
 </script>
 
 <SettingsCard collapsible={false}>
-    {#snippet icon()}
-        <Connection  class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
-    {/snippet}
-    {#snippet title()}
-        <span >I<sup>2</sup>C</span>
-    {/snippet}
+  {#snippet icon()}
+    <Connection class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
+  {/snippet}
+  {#snippet title()}
+    <span>I<sup>2</sup>C</span>
+  {/snippet}
+  {#snippet right()}
+    <button class="btn btn-primary" onclick={triggerScan} disabled={isLoading}>
+      {#if isLoading}
+        <span class="loading loading-ring loading-xs"></span>
+      {:else}
+        Scan
+      {/if}
+    </button>
+  {/snippet}
 
-    <div class="grid">
-        {#if active_devices.length === 0}
-            <div>No I2C devices found</div>
-        {:else}
-            {#each active_devices as device}
-                <div>[{device.address.toString(16)}] {device.part_number} - {device.name}</div>
-            {/each}
-        {/if}
-    </div>
+  <I2CSetting />
+
+  <div class="grid">
+    {#if active_devices.length === 0}
+      <div>No I2C devices found</div>
+    {:else}
+      {#each active_devices as device}
+        <div>[{device.address.toString(16)}] {device.part_number} - {device.name}</div>
+      {/each}
+    {/if}
+  </div>
 </SettingsCard>

app/src/routes/peripherals/i2c/i2cSetting.svelte

@@ -0,0 +1,99 @@
+<script lang="ts">
+  import { Cancel, Edit, EditOff, Power } from '$lib/components/icons'
+  import { socket } from '$lib/stores'
+  import type { PeripheralsConfiguration } from '$lib/types/models'
+  import { onMount } from 'svelte'
+  import { modals } from 'svelte-modals'
+  import ConfirmDialog from '$lib/components/ConfirmDialog.svelte'
+
+  let settings: PeripheralsConfiguration | null = $state(null)
+  let isEditing = $state(false)
+
+  onMount(() => {
+    socket.on('peripheralSettings', handleSettings)
+    socket.sendEvent('peripheralSettings', '')
+    return () => socket.off('peripheralSettings', handleSettings)
+  })
+
+  const handleSettings = (data: any) => {
+    settings = data
+  }
+
+  const handleSave = () => {
+    modals.open(ConfirmDialog, {
+      title: 'Confirm configuration',
+      message:
+        'Are you sure you want to save this configuration? The operation cannot be undone. Please make sure you have the correct settings.',
+      labels: {
+        cancel: { label: 'Cancel', icon: Cancel },
+        confirm: { label: 'Confirm', icon: Power }
+      },
+      onConfirm: () => {
+        modals.close()
+        socket.sendEvent('peripheralSettings', settings)
+      }
+    })
+  }
+
+  const Icon = $derived(isEditing ? EditOff : Edit)
+</script>
+
+{#if settings}
+  <div class="collapse bg-base-100 border-base-300 border">
+    <input type="checkbox" />
+    <div class="collapse-title font-semibold">Configuration</div>
+    <div class="collapse-content text-sm">
+      <div class="flex flex-col gap-2">
+        <label for="sda" class="input validator">
+          SDA
+
+          <input
+            id="sda"
+            type="number"
+            required
+            placeholder="Type a number between 1 to 48"
+            min="0"
+            max="48"
+            title="SDA pin number (0-48)"
+            disabled={!isEditing}
+            bind:value={settings.sda} />
+        </label>
+        <label for="scl" class="input validator">
+          SCL
+
+          <input
+            id="scl"
+            type="number"
+            required
+            placeholder="Type a number between 1 to 48"
+            min="1"
+            max="48"
+            title="SCL pin number (0-48)"
+            disabled={!isEditing}
+            bind:value={settings.scl} />
+        </label>
+        <label class="input validator" for="frequency">
+          Frequency
+          <input
+            id="frequency"
+            type="number"
+            required
+            placeholder="Type a number between 100000 to 430000"
+            min="100000"
+            max="430000"
+            title="I2C frequency in Hz"
+            disabled={!isEditing}
+            bind:value={settings.frequency} />
+        </label>
+        <div>
+          <button class="btn btn-outline btn-primary" onclick={() => (isEditing = !isEditing)}>
+            <Icon class="h-6 w-6" />
+          </button>
+          {#if isEditing}
+            <button class="btn btn-outline btn-primary" onclick={handleSave}>Save</button>
+          {/if}
+        </div>
+      </div>
+    </div>
+  </div>
+{/if}

app/src/routes/peripherals/imu/imu.svelte

@@ -1,310 +1,253 @@
 <script lang="ts">
-	import SettingsCard from "$lib/components/SettingsCard.svelte";
-    import { imu } from '$lib/stores/imu';
-    import { Chart, registerables } from 'chart.js';
-	import { cubicOut } from "svelte/easing";
-	import { slide } from "svelte/transition";
-	import { onDestroy, onMount } from "svelte";
-	import { daisyColor } from "$lib/utilities";
-	import { socket } from "$lib/stores";
-	import type { IMU } from "$lib/types/models";
-	import { useFeatureFlags } from "$lib/stores/featureFlags";
-	import { Rotate3d } from "$lib/components/icons";
+  import SettingsCard from '$lib/components/SettingsCard.svelte'
+  import { imu } from '$lib/stores/imu'
+  import { Chart, registerables } from 'chart.js'
+  import { cubicOut } from 'svelte/easing'
+  import { slide } from 'svelte/transition'
+  import { onDestroy, onMount } from 'svelte'
+  import { socket } from '$lib/stores'
+  import type { IMU } from '$lib/types/models'
+  import { useFeatureFlags } from '$lib/stores/featureFlags'
+  import { Rotate3d } from '$lib/components/icons'
 
-    const features = useFeatureFlags();
+  Chart.register(...registerables)
 
-    Chart.register(...registerables);
+  const features = useFeatureFlags()
+  let intervalId: number
 
-    let angleChartElement: HTMLCanvasElement = $state();
-	let angleChart: Chart;
+  let angleChartElement: HTMLCanvasElement = $state()
+  let tempChartElement: HTMLCanvasElement = $state()
+  let altitudeChartElement: HTMLCanvasElement = $state()
 
-    let tempChartElement: HTMLCanvasElement = $state();
-	let tempChart: Chart;
+  let angleChart: Chart
+  let tempChart: Chart
+  let altitudeChart: Chart
 
-    let altitudeChartElement: HTMLCanvasElement = $state();
-	let altitudeChart: Chart;
-
-    const handleImu = (data: IMU) => {
-        console.log(data);
-        
-        imu.addData(data);   
+  const getChartColors = () => {
+    const style = getComputedStyle(document.body)
+    return {
+      primary: style.getPropertyValue('--color-primary'),
+      secondary: style.getPropertyValue('--color-secondary'),
+      accent: style.getPropertyValue('--color-accent'),
+      background: style.getPropertyValue('--color-background')
     }
+  }
 
-    onMount(() => {
-        socket.on('imu', handleImu);
-        angleChart = new Chart(angleChartElement, {
-			type: 'line',
-			data: {
-				datasets: [
-					{
-						label: 'x',
-						borderColor: daisyColor('--p'),
-						backgroundColor: daisyColor('--p', 50),
-						borderWidth: 2,
-						data: $imu.x,
-						yAxisID: 'y'
-					},
-					{
-						label: 'y',
-						borderColor: daisyColor('--s'),
-						backgroundColor: daisyColor('--s', 50),
-						borderWidth: 2,
-						data: $imu.y,
-						yAxisID: 'y'
-					},
-                    {
-						label: 'z',
-						borderColor: daisyColor('--a'),
-						backgroundColor: daisyColor('--a', 50),
-						borderWidth: 2,
-						data: $imu.z,
-						yAxisID: 'y'
-					}
-				]
-			},
-			options: {
-				maintainAspectRatio: false,
-				responsive: true,
-				plugins: {
-					legend: {
-						display: true
-					},
-					tooltip: {
-						mode: 'index',
-						intersect: false
-					}
-				},
-				elements: {
-					point: {
-						radius: 1
-					}
-				},
-				scales: {
-					x: {
-						grid: {
-							color: daisyColor('--bc', 10)
-						},
-						ticks: {
-							color: daisyColor('--bc')
-						},
-						display: false
-					},
-					y: {
-						type: 'linear',
-						title: {
-							display: true,
-							text: 'Angle [°]',
-							color: daisyColor('--bc'),
-							font: {
-								size: 16,
-								weight: 'bold'
-							}
-						},
-						position: 'left',
-						min:  0,
-						max: 10,
-						grid: { color: daisyColor('--bc', 10) },
-						ticks: { color: daisyColor('--bc') },
-						border: { color: daisyColor('--bc', 10) }
-					}
-				}
-			}
-		});
-        tempChart = new Chart(tempChartElement, {
-			type: 'line',
-			data: {
-				datasets: [
-					{
-						label: 'Barometer temperature',
-						borderColor: daisyColor('--s'),
-						backgroundColor: daisyColor('--s', 50),
-						borderWidth: 2,
-						data: $imu.bmp_temp,
-						yAxisID: 'y'
-					}
-				]
-			},
-			options: {
-				maintainAspectRatio: false,
-				responsive: true,
-				plugins: {
-					legend: {
-						display: true
-					},
-					tooltip: {
-						mode: 'index',
-						intersect: false
-					}
-				},
-				elements: {
-					point: {
-						radius: 1
-					}
-				},
-				scales: {
-					x: {
-						grid: {
-							color: daisyColor('--bc', 10)
-						},
-						ticks: {
-							color: daisyColor('--bc')
-						},
-						display: false
-					},
-					y: {
-						type: 'linear',
-						title: {
-							display: true,
-							text: 'Temperature [C°]',
-							color: daisyColor('--bc'),
-							font: {
-								size: 16,
-								weight: 'bold'
-							}
-						},
-						position: 'left',
-						min:  0,
-						max: 10,
-						grid: { color: daisyColor('--bc', 10) },
-						ticks: { color: daisyColor('--bc') },
-						border: { color: daisyColor('--bc', 10) }
-					}
-				}
-			}
-		});
-        altitudeChart = new Chart(altitudeChartElement, {
-			type: 'line',
-			data: {
-				datasets: [
-					{
-						label: 'Altitude',
-						borderColor: daisyColor('--p'),
-						backgroundColor: daisyColor('--p', 50),
-						borderWidth: 2,
-						data: $imu.altitude,
-						yAxisID: 'y'
-					}
-				]
-			},
-			options: {
-				maintainAspectRatio: false,
-				responsive: true,
-				plugins: {
-					legend: {
-						display: true
-					},
-					tooltip: {
-						mode: 'index',
-						intersect: false
-					}
-				},
-				elements: {
-					point: {
-						radius: 1
-					}
-				},
-				scales: {
-					x: {
-						grid: {
-							color: daisyColor('--bc', 10)
-						},
-						ticks: {
-							color: daisyColor('--bc')
-						},
-						display: false
-					},
-					y: {
-						type: 'linear',
-						title: {
-							display: true,
-							text: 'Altitude [M]',
-							color: daisyColor('--bc'),
-							font: {
-								size: 16,
-								weight: 'bold'
-							}
-						},
-						position: 'left',
-						min:  0,
-						max: 10,
-						grid: { color: daisyColor('--bc', 10) },
-						ticks: { color: daisyColor('--bc') },
-						border: { color: daisyColor('--bc', 10) }
-					}
-				}
-			}
-		});
-        setInterval(() => {
-			updateData(), 200;
-		});
+  const createBaseChartConfig = (bgColor: string) => ({
+    maintainAspectRatio: false,
+    responsive: true,
+    plugins: {
+      legend: { display: true },
+      tooltip: { mode: 'index', intersect: false }
+    },
+    elements: { point: { radius: 1 } },
+    scales: {
+      x: {
+        grid: { color: bgColor },
+        ticks: { color: bgColor },
+        display: false
+      },
+      y: {
+        type: 'linear',
+        position: 'left',
+        min: 0,
+        max: 10,
+        grid: { color: bgColor },
+        ticks: { color: bgColor },
+        border: { color: bgColor }
+      }
+    }
+  })
+
+  const initializeCharts = () => {
+    const colors = getChartColors()
+    const baseConfig = createBaseChartConfig(colors.background)
+
+    angleChart = new Chart(angleChartElement, {
+      type: 'line',
+      data: {
+        datasets: [
+          {
+            label: 'x',
+            borderColor: colors.primary,
+            backgroundColor: colors.primary,
+            borderWidth: 2,
+            data: $imu.x,
+            yAxisID: 'y'
+          },
+          {
+            label: 'y',
+            borderColor: colors.secondary,
+            backgroundColor: colors.secondary,
+            borderWidth: 2,
+            data: $imu.y,
+            yAxisID: 'y'
+          },
+          {
+            label: 'z',
+            borderColor: colors.accent,
+            backgroundColor: colors.accent,
+            borderWidth: 2,
+            data: $imu.z,
+            yAxisID: 'y'
+          }
+        ]
+      },
+      options: {
+        ...baseConfig,
+        scales: {
+          ...baseConfig.scales,
+          y: {
+            ...baseConfig.scales.y,
+            title: {
+              display: true,
+              text: 'Angle [°]',
+              color: colors.background,
+              font: { size: 16, weight: 'bold' }
+            }
+          }
+        }
+      }
     })
 
-    onDestroy(() => {
-        socket.off('imu', handleImu);
+    tempChart = new Chart(tempChartElement, {
+      type: 'line',
+      data: {
+        datasets: [
+          {
+            label: 'Barometer temperature',
+            borderColor: colors.secondary,
+            backgroundColor: colors.secondary,
+            borderWidth: 2,
+            data: $imu.bmp_temp,
+            yAxisID: 'y'
+          }
+        ]
+      },
+      options: {
+        ...baseConfig,
+        scales: {
+          ...baseConfig.scales,
+          y: {
+            ...baseConfig.scales.y,
+            title: {
+              display: true,
+              text: 'Temperature [C°]',
+              color: colors.background,
+              font: { size: 16, weight: 'bold' }
+            }
+          }
+        }
+      }
     })
 
-    const updateData = () => {
-        if ($features.imu) {
-            angleChart.data.labels = $imu.x;
-            angleChart.data.datasets[0].data = $imu.x;
-            angleChart.data.datasets[1].data = $imu.y;
-            angleChart.data.datasets[2].data = $imu.z;
-            angleChart.options.scales!.y!.min = Math.min(Math.min(...$imu.x), Math.min(...$imu.y), Math.min(...$imu.z)) - 1;
-            angleChart.options.scales!.y!.max = Math.max(Math.max(...$imu.x), Math.max(...$imu.y), Math.max(...$imu.z)) + 1;
-            angleChart.update('none');
+    altitudeChart = new Chart(altitudeChartElement, {
+      type: 'line',
+      data: {
+        datasets: [
+          {
+            label: 'Altitude',
+            borderColor: colors.primary,
+            backgroundColor: colors.primary,
+            borderWidth: 2,
+            data: $imu.altitude,
+            yAxisID: 'y'
+          }
+        ]
+      },
+      options: {
+        ...baseConfig,
+        scales: {
+          ...baseConfig.scales,
+          y: {
+            ...baseConfig.scales.y,
+            title: {
+              display: true,
+              text: 'Altitude [M]',
+              color: colors.background,
+              font: { size: 16, weight: 'bold' }
+            }
+          }
         }
+      }
+    })
+  }
 
-        if ($features.bmp) {
-            tempChart.data.labels = $imu.bmp_temp;
-            tempChart.data.datasets[0].data = $imu.bmp_temp;
-            tempChart.options.scales!.y!.min = Math.min(...$imu.bmp_temp) - 1;
-            tempChart.options.scales!.y!.max = Math.max(...$imu.bmp_temp) + 1;
-            tempChart.update('none');
-    
-            altitudeChart.data.labels = $imu.altitude;
-            altitudeChart.data.datasets[0].data = $imu.altitude;
-            altitudeChart.options.scales!.y!.min = Math.min(Math.min(...$imu.altitude)) - 1;
-            altitudeChart.options.scales!.y!.max = Math.max(Math.max(...$imu.altitude)) + 1;
-            altitudeChart.update('none');
-        }
+  const updateChartData = (chart: Chart, data: number[], label: string) => {
+    chart.data.labels = data
+    chart.data.datasets[0].data = data
+    chart.options.scales!.y!.min = Math.min(...data) - 1
+    chart.options.scales!.y!.max = Math.max(...data) + 1
+    chart.update('none')
+  }
+
+  const updateData = () => {
+    if ($features.imu) {
+      angleChart.data.labels = $imu.x
+      angleChart.data.datasets[0].data = $imu.x
+      angleChart.data.datasets[1].data = $imu.y
+      angleChart.data.datasets[2].data = $imu.z
+
+      const allValues = [...$imu.x, ...$imu.y, ...$imu.z]
+      angleChart.options.scales!.y!.min = Math.min(...allValues) - 1
+      angleChart.options.scales!.y!.max = Math.max(...allValues) + 1
+      angleChart.update('none')
     }
 
+    if ($features.bmp) {
+      updateChartData(tempChart, $imu.bmp_temp, 'Temperature')
+      updateChartData(altitudeChart, $imu.altitude, 'Altitude')
+    }
+  }
+
+  onMount(() => {
+    socket.on('imu', (data: IMU) => {
+      console.log(data)
+      imu.addData(data)
+    })
+
+    initializeCharts()
+    intervalId = setInterval(updateData, 200)
+  })
+
+  onDestroy(() => {
+    socket.off('imu')
+    clearInterval(intervalId)
+  })
 </script>
 
 <SettingsCard collapsible={false}>
-    {#snippet icon()}
-		<Rotate3d  class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
-	{/snippet}
-    {#snippet title()}
-		<span >IMU</span>
-	{/snippet}
-    {#if $features.imu} 
-        <div class="w-full overflow-x-auto">
-            <div
-            class="flex w-full flex-col space-y-1 h-60"
-            transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-                <canvas bind:this={angleChartElement}></canvas>
-            </div>
-        </div>
-    {/if}
-    {#if $features.bmp} 
-        <div class="w-full overflow-x-auto">
-            <div
-                class="flex w-full flex-col space-y-1 h-60"
-                transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-                <canvas bind:this={tempChartElement}></canvas>
-            </div>
-        </div>
-        <div class="w-full overflow-x-auto">
-            <div
-            class="flex w-full flex-col space-y-1 h-60"
-            transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-            <canvas bind:this={altitudeChartElement}></canvas>
-        </div>
+  {#snippet icon()}
+    <Rotate3d class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
+  {/snippet}
+  {#snippet title()}
+    <span>IMU</span>
+  {/snippet}
+
+  {#if $features.imu}
+    <div class="w-full overflow-x-auto">
+      <div
+        class="flex w-full flex-col space-y-1 h-60"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <canvas bind:this={angleChartElement}></canvas>
+      </div>
     </div>
-    {/if}
-    <!-- <IMUSetting /> -->
-</SettingsCard>
+  {/if}
+
+  {#if $features.bmp}
+    <div class="w-full overflow-x-auto">
+      <div
+        class="flex w-full flex-col space-y-1 h-60"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <canvas bind:this={tempChartElement}></canvas>
+      </div>
+    </div>
+    <div class="w-full overflow-x-auto">
+      <div
+        class="flex w-full flex-col space-y-1 h-60"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <canvas bind:this={altitudeChartElement}></canvas>
+      </div>
+    </div>
+  {/if}
+</SettingsCard>

app/src/routes/peripherals/servo/+page.svelte

@@ -1,9 +1,12 @@
 <script lang="ts">
-    import Servos from './servos.svelte';
-    import ServoTable from './ServoTable.svelte';
+  import Servos from './servos.svelte'
+  import ServoTable from './ServoTable.svelte'
+
+  let servoId = $state(0)
+  let pwm = $state(306)
 </script>
 
 <div class="mx-0 my-1 flex flex-col space-y-4 sm:mx-8 sm:my-8">
-    <Servos />
-    <ServoTable />
+  <Servos bind:servoId bind:pwm />
+  <ServoTable {servoId} {pwm} />
 </div>

app/src/routes/peripherals/servo/ServoTable.svelte

@@ -1,73 +1,113 @@
 <script lang="ts">
-    import { api } from '$lib/api';
-    import { onMount } from 'svelte';
-    interface Props {
-        data?: any;
+  import { api } from '$lib/api'
+  import { onMount } from 'svelte'
+  import { RotateCw, RotateCcw } from '$lib/components/icons'
+  interface Props {
+    data?: any
+    servoId?: number
+    pwm?: number
+  }
+
+  let {
+    data = $bindable({
+      servos: []
+    }),
+    pwm = $bindable(306),
+    servoId = $bindable(0)
+  }: Props = $props()
+
+  const updateValue = (event: Event, index: number, key: string) => {
+    data.servos[index][key] = Number((event.target as HTMLInputElement).value)
+  }
+
+  const syncConfig = async () => {
+    await api.post('/api/servo/config', data)
+  }
+
+  const toggleDirection = async (index: number) => {
+    data.servos[index].direction = data.servos[index].direction === 1 ? -1 : 1
+    await syncConfig()
+  }
+
+  onMount(async () => {
+    const result = await api.get('/api/servo/config')
+    if (result.isOk()) {
+      data = result.inner
     }
+  })
 
-    let { data = $bindable({
-        servos: []
-    }) }: Props = $props();
-
-    const updateValue = (event, index, key) => {
-        data.servos[index][key] = event.target.innerText;
-    };
-
-    const syncConfig = async () => {
-        await api.post('/api/servo/config', data);
-    };
-
-    onMount(async () => {
-        const result = await api.get('/api/servo/config');
-        if (result.isOk()) {
-            data = result.inner;
-        }
-    });
+  const setCenterPWM = async () => {
+    console.log('setCenterPWM', servoId, pwm)
+    data.servos[servoId]['center_pwm'] = pwm
+    await syncConfig()
+  }
 </script>
 
-<div class="overflow-x-auto">
-    <table class="table table-xs">
-        <thead>
-            <tr>
-                <th>Center PWM</th>
-                <th>Center Angle</th>
-                <th>Direction</th>
-                <th>Conversion</th>
-            </tr>
-        </thead>
-        <tbody>
-            {#each data.servos as servo, index}
-                <tr>
-                    <td
-                        contenteditable="true"
-                        onblur={syncConfig}
-                        oninput={event => updateValue(event, index, 'center_pwm')}
-                    >
-                        {servo.center_pwm}
-                    </td>
-                    <td
-                        contenteditable="true"
-                        onblur={syncConfig}
-                        oninput={event => updateValue(event, index, 'center_angle')}
-                    >
-                        {servo.center_angle}
-                    </td>
-                    <td
-                        contenteditable="true"
-                        onblur={syncConfig}
-                        oninput={event => updateValue(event, index, 'direction')}
-                    >
-                        {servo.direction}
-                    </td>
-                    <td
-                        contenteditable="true"
-                        onblur={syncConfig}
-                        oninput={event => updateValue(event, index, 'conversion')}
-                    >
-                        {servo.conversion}
-                    </td>
-                </tr>
-            {/each}
-        </tbody>
-    </table>
+<div>
+  <button class="btn btn-sm btn-primary" onclick={() => setCenterPWM()}>Set center pwm</button>
+</div>
+
+<div class="overflow-x-auto">
+  <table class="table table-xs">
+    <thead>
+      <tr>
+        <th>Servo</th>
+        <th>Center PWM</th>
+        <th>Center Angle</th>
+        <th>Direction</th>
+        <th>Conversion</th>
+      </tr>
+    </thead>
+    <tbody>
+      {#each data.servos as servo, index}
+        <tr class="hover:bg-base-200">
+          <td class="font-medium">Servo {index}</td>
+          <td>
+            <input
+              type="number"
+              class="input input-sm input-bordered w-20"
+              value={servo.center_pwm}
+              onblur={syncConfig}
+              oninput={event => updateValue(event, index, 'center_pwm')}
+              min="80"
+              max="600" />
+          </td>
+          <td>
+            <input
+              type="number"
+              step="0.1"
+              class="input input-sm input-bordered w-20"
+              value={servo.center_angle}
+              onblur={syncConfig}
+              oninput={event => updateValue(event, index, 'center_angle')}
+              min="-90"
+              max="90" />
+          </td>
+          <td>
+            <button
+              class="btn btn-sm btn-ghost"
+              title="Toggle direction {servo.direction}"
+              onclick={() => toggleDirection(index)}>
+              {#if servo.direction === 1}
+                <RotateCw class="w-4 h-4 text-green-500" />
+              {:else}
+                <RotateCcw class="w-4 h-4" />
+              {/if}
+            </button>
+          </td>
+          <td>
+            <input
+              type="number"
+              step="0.01"
+              class="input input-sm input-bordered w-20"
+              value={servo.conversion}
+              onblur={syncConfig}
+              oninput={event => updateValue(event, index, 'conversion')}
+              min="0"
+              max="10" />
+          </td>
+        </tr>
+      {/each}
+    </tbody>
+  </table>
 </div>

app/src/routes/peripherals/servo/servos.svelte

@@ -1,75 +1,63 @@
 <script lang="ts">
-    import SettingsCard from '$lib/components/SettingsCard.svelte';
-    import type { ServoConfiguration, Servo } from '$lib/types/models';
-    import Spinner from '$lib/components/Spinner.svelte';
+  import { socket } from '$lib/stores'
+  import { throttler as Throttler } from '$lib/utilities'
 
-    import { socket } from '$lib/stores';
-    import { onDestroy, onMount } from 'svelte';
-    import { throttler as Throttler } from '$lib/utilities';
-    import { MotorOutline } from '$lib/components/icons';
+  let { servoId = $bindable(0), pwm = $bindable(306) } = $props()
 
-    let isLoading = false;
+  let active = $state(false)
 
-    let active = $state(false);
+  let allServos = $state(false)
 
-    let servoId = $state(0);
+  const throttler = new Throttler()
 
-    const throttler = new Throttler();
+  const activateServo = () => {
+    socket.sendEvent('servoState', { active: 1 })
+  }
 
-    const sweep = (event: any) => {
-        let channel = event.detail.channel;
-        socket.sendEvent('servoConfiguration', { servos: [{ channel, sweep: true }] });
-    };
+  const deactivateServo = () => {
+    socket.sendEvent('servoState', { active: 0 })
+  }
 
-    const activateServo = (event: any) => {
-        socket.sendEvent('servoState', { active: 1 });
-    };
+  const updatePWM = () => {
+    throttler.throttle(() => {
+      socket.sendEvent('servoPWM', { servo_id: servoId, pwm })
+    }, 10)
+  }
 
-    const deactivateServo = (event: any) => {
-        socket.sendEvent('servoState', { active: 0 });
-    };
-
-    let pwm = $state(306);
-
-    const updatePWM = () => {
-        throttler.throttle(() => {
-            socket.sendEvent('servoPWM', { servo_id: servoId, pwm });
-        }, 10);
-    };
+  const toggleMode = () => {
+    servoId = allServos ? -1 : 0
+  }
 </script>
 
-<SettingsCard collapsible={false}>
-    {#snippet icon()}
-        <MotorOutline  class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
-    {/snippet}
-    {#snippet title()}
-        <span >Servo</span>
-    {/snippet}
-    {pwm}
-    <input
-        type="range"
-        min="80"
-        max="600"
-        bind:value={pwm}
-        oninput={updatePWM}
-        class="w-full h-2 bg-gray-200 rounded-lg appearance-none cursor-pointer dark:bg-gray-700"
-    />
+<div class="flex flex-col">
+  <h2 class="text-lg">General servo configuration</h2>
+  <span>Servo</span>
+  <span>{pwm}</span>
+</div>
+<input
+  type="range"
+  min="80"
+  max="600"
+  bind:value={pwm}
+  oninput={updatePWM}
+  class="w-full h-2 bg-gray-200 rounded-lg appearance-none cursor-pointer dark:bg-gray-700" />
 
-    {#if isLoading}
-        <Spinner />
-    {:else}
-        <div class="flex flex-col">
-            <h2 class="text-lg">General servo configuration</h2>
-            <span class="flex items-center gap-2">
-                <label for="servoId">Servo active {servoId}</label>
-                <input type="range" min="0" max="11" step="1" bind:value={servoId} />
-                <input
-                    type="checkbox"
-                    class="toggle"
-                    bind:checked={active}
-                    onchange={active ? activateServo : deactivateServo}
-                />
-            </span>
-        </div>
-    {/if}
-</SettingsCard>
+<div class="flex flex-col">
+  <h2 class="text-lg">General servo configuration</h2>
+  <span>
+    <label for="mode">All servoes</label>
+    <input type="checkbox" class="toggle" bind:checked={allServos} onchange={toggleMode} />
+  </span>
+  <span>
+    <label for="active">Active</label>
+    <input
+      type="checkbox"
+      class="toggle"
+      bind:checked={active}
+      onchange={active ? activateServo : deactivateServo} />
+  </span>
+  <span class="flex items-center gap-2">
+    <label for="servoId">Servo active {servoId}</label>
+    <input type="range" min="0" max="11" step="1" bind:value={servoId} />
+  </span>
+</div>

app/src/routes/system/filesystem/File.svelte

@@ -1,11 +1,24 @@
-<script>
-  import { FileIcon } from '$lib/components/icons'
+<script lang="ts">
+  import { FileIcon, TrashIcon } from '$lib/components/icons'
 
-  let { name, selected } = $props()
+  interface Props {
+    name: string
+    selected: (name: string) => void
+    onDelete: (name: string) => void
+  }
+
+  let { name, selected, onDelete }: Props = $props()
 </script>
 
-<!-- svelte-ignore a11y_interactive_supports_focus -->
-<!-- svelte-ignore a11y_click_events_have_key_events -->
-<span role="button" class="flex pl-4 gap-2 items-center" onclick={selected}>
-  <FileIcon />{name}
-</span>
+<div class="flex items-center pl-4 group hover:bg-gray-700 rounded py-1">
+  <button class="flex items-center gap-2 flex-grow" onclick={() => selected(name)}>
+    <FileIcon class="w-4 h-4" />
+    <span class="text-sm">{name}</span>
+  </button>
+
+  <button
+    class="opacity-0 group-hover:opacity-100 p-1 hover:text-red-500"
+    onclick={() => onDelete(name)}>
+    <TrashIcon class="w-4 h-4" />
+  </button>
+</div>

app/src/routes/system/filesystem/FileSystem.svelte

@@ -1,62 +1,172 @@
 <script lang="ts">
-	import SettingsCard from "$lib/components/SettingsCard.svelte";
-	import Spinner from "$lib/components/Spinner.svelte";
-	import Folder from "./Folder.svelte";
-	import { api } from "$lib/api";
-	import type { Directory } from "$lib/types/models";
-	import { FolderIcon } from "$lib/components/icons";
+  import SettingsCard from '$lib/components/SettingsCard.svelte'
+  import Spinner from '$lib/components/Spinner.svelte'
+  import Folder from './Folder.svelte'
+  import { api } from '$lib/api'
+  import type { Directory } from '$lib/types/models'
+  import { FolderIcon, Add, FileIcon } from '$lib/components/icons'
+  import { modals } from 'svelte-modals'
+  import NewFolderDialog from './NewFolderDialog.svelte'
+  import NewFileDialog from './NewFileDialog.svelte'
 
-    let filename = $state('');
+  let filename = $state('')
+  let content = $state('')
+  let isEditing = $state(false)
 
-    const getFiles = async () => {
-        const result = await api.get<Directory>('/api/files')
-        if (result.isOk()) {
-            return result.inner;
-        }
-        return { root: {} }
-    };
-
-    const getContent = async (name: string) => {
-        if (!name) return '';
-        const result = await api.get(`/api/config/${name}`)
-        if (result.isOk()) {
-            return JSON.stringify(result.inner, null, 4);
-        }
-        return ''
+  const getFiles = async () => {
+    const result = await api.get<Directory>('/api/files')
+    if (result.isOk()) {
+      return result.inner
     }
+    return { root: {} }
+  }
 
-    const deleteFile = async (name: string) => {
-        const result = await api.post(`/api/files/delete`, { file: "/config/"+ name })
-        if (result.isOk()) {
-            return result.inner;
-        }
-        return ''
+  const getContent = async (name: string) => {
+    if (!name) return ''
+    const result = await api.get(`/api/config/${name}`)
+    if (result.isOk()) {
+      content = JSON.stringify(result.inner, null, 4)
+      return content
     }
+    return ''
+  }
 
-    const updateSelected = async (event:any) => {
-        filename = event.detail.name;
+  const saveContent = async () => {
+    if (!filename) return
+    const result = await api.post('/api/files/edit', {
+      file: '/config/' + filename,
+      content
+    })
+    if (result.isOk()) {
+      isEditing = false
     }
+  }
+
+  const deleteFile = async (name: string) => {
+    if (!confirm(`Are you sure you want to delete ${name}?`)) return
+    const result = await api.post('/api/files/delete', { file: '/config/' + name })
+    if (result.isOk()) {
+      filename = ''
+      content = ''
+    }
+  }
+
+  const createFolder = async (folderName: string) => {
+    if (!folderName) return
+    const result = await api.post('/api/files/mkdir', {
+      path: '/config/' + folderName
+    })
+    if (result.isOk()) {
+      // Refresh the file list
+      await getFiles()
+    }
+  }
+
+  const updateSelected = async (name: string) => {
+    filename = name
+    isEditing = false
+    await getContent(name)
+  }
+
+  const openNewFolderDialog = () => {
+    modals.open(NewFolderDialog, {
+      onConfirm: createFolder
+    })
+  }
+
+  const createFile = async (fileName: string) => {
+    if (!fileName) return
+    const result = await api.post('/api/files/edit', {
+      file: '/config/' + fileName,
+      content: '{}' // Default empty JSON object
+    })
+    if (result.isOk()) {
+      // Refresh the file list and select the new file
+      await getFiles()
+      await updateSelected(fileName)
+    }
+  }
+
+  const openNewFileDialog = () => {
+    modals.open(NewFileDialog, {
+      onConfirm: createFile
+    })
+  }
 </script>
-<SettingsCard collapsible={false}>
-    {#snippet icon()}
-        <FolderIcon  class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
-    {/snippet}
-	{#snippet title()}
-        <span >File System</span>
-    {/snippet}
-    <div class="w-full overflow-x-auto">
-		{#await getFiles()}
-			<Spinner />
-		{:then files}
-            <Folder name="/" files={files.root} expanded on:selected={updateSelected}/>
-        {/await}
-            
-        {#await getContent(filename)}
-            <div>
-                <Spinner />
-            </div>
-        {:then content}
-            <pre>{content}</pre>
-        {/await}
-    </div>
-</SettingsCard>
+
+<!-- <SettingsCard collapsible={false}> -->
+<!-- {#snippet icon()} -->
+<FolderIcon class="flex-shrink-0 mr-2 h-6 w-6 self-end" />
+<!-- {/snippet}
+  {#snippet title()} -->
+<div class="flex justify-between items-center w-full gap-2">
+  <span>File System</span>
+  <div class="flex gap-2">
+    <button class="btn btn-sm btn-primary flex items-center gap-2" onclick={openNewFileDialog}>
+      <FileIcon class="w-4 h-4" />
+      New File
+    </button>
+    <button class="btn btn-sm btn-primary flex items-center gap-2" onclick={openNewFolderDialog}>
+      <Add class="w-4 h-4" />
+      New Folder
+    </button>
+  </div>
+</div>
+<!-- {/snippet} -->
+
+<div class="flex flex-col md:flex-row gap-4 w-full">
+  <!-- File Tree -->
+  <div
+    class="w-full md:w-[300px] md:min-w-[300px] md:max-w-[300px] border-b md:border-b-0 md:border-r pb-4 md:pb-0 md:pr-4">
+    {#await getFiles()}
+      <Spinner />
+    {:then files}
+      <Folder
+        name="/"
+        files={files.root}
+        expanded
+        selected={updateSelected}
+        onDelete={deleteFile} />
+    {/await}
+  </div>
+
+  <!-- File Content -->
+  <div class="flex-1 min-w-0">
+    {#if filename}
+      <div class="flex flex-col sm:flex-row justify-between items-start sm:items-center mb-4 gap-2">
+        <h3 class="text-lg font-semibold truncate">{filename}</h3>
+        <div class="flex gap-2">
+          {#if isEditing}
+            <button class="btn btn-sm btn-primary" onclick={saveContent}>Save</button>
+            <button class="btn btn-sm btn-secondary" onclick={() => (isEditing = false)}>
+              Cancel
+            </button>
+          {:else}
+            <button class="btn btn-sm btn-primary" onclick={() => (isEditing = true)}>
+              Edit
+            </button>
+            <button class="btn btn-sm btn-danger" onclick={() => deleteFile(filename)}>
+              Delete
+            </button>
+          {/if}
+        </div>
+      </div>
+
+      {#await getContent(filename)}
+        <Spinner />
+      {:then _}
+        {#if isEditing}
+          <textarea
+            class="w-full h-[300px] sm:h-[500px] font-mono p-2 bg-gray-800 text-white"
+            bind:value={content}></textarea>
+        {:else}
+          <pre
+            class="bg-gray-800 p-4 rounded overflow-auto max-h-[300px] sm:max-h-[500px]">{content}</pre>
+        {/if}
+      {/await}
+    {:else}
+      <div class="text-center text-gray-500">Select a file to view its contents</div>
+    {/if}
+  </div>
+</div>
+<!-- </SettingsCard> -->

app/src/routes/system/filesystem/Folder.svelte

@@ -1,47 +1,44 @@
 <script lang="ts">
-	import Folder from './Folder.svelte';
-	import File from './File.svelte';
-	import { createEventDispatcher } from 'svelte';
-	import { FolderIcon, FolderOpenOutline } from '$lib/components/icons';
+  import Folder from './Folder.svelte'
+  import File from './File.svelte'
+  import { FolderIcon, FolderOpenOutline } from '$lib/components/icons'
 
-	interface Props {
-		expanded?: boolean;
-		name: any;
-		files: any;
-	}
+  interface Props {
+    expanded?: boolean
+    name: string
+    files: any
+    selected: (name: string) => void
+    onDelete: (name: string) => void
+  }
 
-	let { expanded = $bindable(false), name, files }: Props = $props();
+  let { expanded = $bindable(false), name, files, selected, onDelete }: Props = $props()
 
-	function toggle() {
-		expanded = !expanded;
-	}
-
-    const dispatch = createEventDispatcher();
-
-    const updateSelected = async (event:any) => {
-        dispatch('selected', { name:event.detail.name });
-    }
+  function toggle() {
+    expanded = !expanded
+  }
 </script>
 
-<button class="flex pl-2" onclick={toggle}>
+<div class="folder-item">
+  <button class="flex items-center pl-2 hover:bg-gray-700 w-full rounded py-1" onclick={toggle}>
     {#if expanded}
-        <FolderOpenOutline class="w-6 h-6" />
+      <FolderOpenOutline class="w-5 h-5 mr-1" />
     {:else}
-        <FolderIcon class="w-6 h-6" />
+      <FolderIcon class="w-5 h-5 mr-1" />
     {/if}
-    {name}
-</button>
+    <span class="text-sm">{name}</span>
+  </button>
 
-{#if expanded}
-    <ul class="ml-5 border-l border-slate-600">
-        {#each Object.entries(files) as [name, content]}
-            <li class="p-1">
-                {#if typeof content == 'object'}
-					<Folder {name} files={content} on:selected={updateSelected} />
-				{:else}
-					<File {name} on:selected={updateSelected}/>
-				{/if}
-			</li>
-		{/each}
-	</ul>
-{/if}
+  {#if expanded}
+    <ul class="ml-4 border-l border-gray-600 mt-1">
+      {#each Object.entries(files) as [itemName, content]}
+        <li class="py-1">
+          {#if typeof content === 'object'}
+            <Folder name={itemName} files={content} {selected} {onDelete} />
+          {:else}
+            <File name={itemName} {selected} {onDelete} />
+          {/if}
+        </li>
+      {/each}
+    </ul>
+  {/if}
+</div>

app/src/routes/system/filesystem/NewFileDialog.svelte

@@ -0,0 +1,44 @@
+<script lang="ts">
+  import { focusTrap } from 'svelte-focus-trap'
+  import { fly } from 'svelte/transition'
+  import { exitBeforeEnter, modals, type ModalProps } from 'svelte-modals'
+  import { Cancel, Check } from '$lib/components/icons'
+
+  let { isOpen, onConfirm }: ModalProps = $props()
+  let fileName = $state('')
+
+  const handleCreate = () => {
+    if (!fileName) return
+    onConfirm(fileName)
+    modals.close()
+  }
+</script>
+
+{#if isOpen}
+  <div
+    role="dialog"
+    class="pointer-events-none fixed inset-0 z-50 flex items-center justify-center"
+    transition:fly={{ y: 50 }}
+    use:exitBeforeEnter
+    use:focusTrap>
+    <div
+      class="rounded-box bg-base-100 pointer-events-auto flex min-w-fit max-w-md flex-col justify-between p-4 shadow-lg">
+      <h2 class="text-base-content text-start text-2xl font-bold">Create New File</h2>
+      <div class="divider my-2"></div>
+      <input
+        type="text"
+        class="input input-bordered w-full"
+        placeholder="File name"
+        bind:value={fileName} />
+      <div class="divider my-2"></div>
+      <div class="flex justify-end gap-2">
+        <button class="btn btn-error inline-flex items-center" onclick={() => modals.close()}>
+          <Cancel class="mr-2 h-5 w-5" /><span>Cancel</span>
+        </button>
+        <button class="btn btn-primary inline-flex items-center" onclick={handleCreate}>
+          <Check class="mr-2 h-5 w-5" /><span>Create</span>
+        </button>
+      </div>
+    </div>
+  </div>
+{/if}

app/src/routes/system/filesystem/NewFolderDialog.svelte

@@ -0,0 +1,44 @@
+<script lang="ts">
+  import { focusTrap } from 'svelte-focus-trap'
+  import { fly } from 'svelte/transition'
+  import { exitBeforeEnter, modals, type ModalProps } from 'svelte-modals'
+  import { Cancel, Check } from '$lib/components/icons'
+
+  let { isOpen, onConfirm }: ModalProps = $props()
+  let folderName = $state('')
+
+  const handleCreate = () => {
+    if (!folderName) return
+    onConfirm(folderName)
+    modals.close()
+  }
+</script>
+
+{#if isOpen}
+  <div
+    role="dialog"
+    class="pointer-events-none fixed inset-0 z-50 flex items-center justify-center"
+    transition:fly={{ y: 50 }}
+    use:exitBeforeEnter
+    use:focusTrap>
+    <div
+      class="rounded-box bg-base-100 pointer-events-auto flex min-w-fit max-w-md flex-col justify-between p-4 shadow-lg">
+      <h2 class="text-base-content text-start text-2xl font-bold">Create New Folder</h2>
+      <div class="divider my-2"></div>
+      <input
+        type="text"
+        class="input input-bordered w-full"
+        placeholder="Folder name"
+        bind:value={folderName} />
+      <div class="divider my-2"></div>
+      <div class="flex justify-end gap-2">
+        <button class="btn btn-error inline-flex items-center" onclick={() => modals.close()}>
+          <Cancel class="mr-2 h-5 w-5" /><span>Cancel</span>
+        </button>
+        <button class="btn btn-primary inline-flex items-center" onclick={handleCreate}>
+          <Check class="mr-2 h-5 w-5" /><span>Create</span>
+        </button>
+      </div>
+    </div>
+  </div>
+{/if}

app/src/routes/system/status/StatusItem.svelte

@@ -1,15 +0,0 @@
-<script lang="ts">
-  const { icon, title, description } = $props()
-
-  const Icon = $derived(icon)
-</script>
-
-<div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2">
-  <div class="mask mask-hexagon bg-primary h-auto w-10 flex-none">
-    <Icon class="text-primary-content h-auto w-full scale-75" />
-  </div>
-  <div>
-    <div class="font-bold">{title}</div>
-    <div class="text-sm opacity-75">{description}</div>
-  </div>
-</div>

app/src/routes/system/status/SystemStatus.svelte

@@ -6,12 +6,10 @@
   import Spinner from '$lib/components/Spinner.svelte'
   import { slide } from 'svelte/transition'
   import { cubicOut } from 'svelte/easing'
-
   import type { SystemInformation, Analytics } from '$lib/types/models'
   import { socket } from '$lib/stores/socket'
   import { api } from '$lib/api'
   import { convertSeconds } from '$lib/utilities'
-
   import { useFeatureFlags } from '$lib/stores/featureFlags'
   import {
     Cancel,
@@ -31,7 +29,7 @@
     Temperature,
     Stopwatch
   } from '$lib/components/icons'
-  import StatusItem from './StatusItem.svelte'
+  import StatusItem from '$lib/components/StatusItem.svelte'
   import ActionButton from './ActionButton.svelte'
 
   const features = useFeatureFlags()
@@ -236,7 +234,7 @@
     {#each actionButtons as button}
       {#if button.condition === undefined || button.condition()}
         <ActionButton
-          on:click={button.onClick}
+          onclick={button.onClick}
           icon={button.icon}
           label={button.label}
           type={button.type || 'primary'} />

app/src/routes/wifi/ap/Accesspoint.svelte

@@ -1,436 +1,364 @@
 <script lang="ts">
-    import { preventDefault } from 'svelte/legacy';
+  import { preventDefault } from 'svelte/legacy'
 
-    import { onMount, onDestroy } from 'svelte';
-    import { slide } from 'svelte/transition';
-    import { cubicOut } from 'svelte/easing';
-    import { PasswordInput } from '$lib/components/input';
-    import SettingsCard from '$lib/components/SettingsCard.svelte';
-    import { notifications } from '$lib/components/toasts/notifications';
-    import Spinner from '$lib/components/Spinner.svelte';
-    import type { ApSettings, ApStatus } from '$lib/types/models';
-    import { api } from '$lib/api';
-    import { useFeatureFlags } from '$lib/stores';
-    import { AP, Devices, Home, MAC } from '$lib/components/icons';
+  import { onMount, onDestroy } from 'svelte'
+  import { slide } from 'svelte/transition'
+  import { cubicOut } from 'svelte/easing'
+  import { PasswordInput } from '$lib/components/input'
+  import SettingsCard from '$lib/components/SettingsCard.svelte'
+  import { notifications } from '$lib/components/toasts/notifications'
+  import Spinner from '$lib/components/Spinner.svelte'
+  import type { ApSettings, ApStatus } from '$lib/types/models'
+  import { api } from '$lib/api'
+  import { useFeatureFlags } from '$lib/stores'
+  import { AP, Devices, Home, MAC } from '$lib/components/icons'
+  import StatusItem from '$lib/components/StatusItem.svelte'
 
-    const features = useFeatureFlags();
+  const features = useFeatureFlags()
 
-    let apSettings: ApSettings = $state();
-    let apStatus: ApStatus = $state();
+  let apSettings: ApSettings = $state()
+  let apStatus: ApStatus = $state()
 
-    let formField: any = $state();
+  let formField: any = $state()
 
-    async function getAPStatus() {
-        const result = await api.get<ApStatus>('/api/wifi/ap/status');
-        if (result.isErr()) {
-            console.error('Error:', result.inner);
-            return;
-        }
-        apStatus = result.inner;
-        return apStatus;
+  async function getAPStatus() {
+    const result = await api.get<ApStatus>('/api/wifi/ap/status')
+    if (result.isErr()) {
+      console.error('Error:', result.inner)
+      return
+    }
+    apStatus = result.inner
+    return apStatus
+  }
+
+  async function getAPSettings() {
+    const result = await api.get<ApSettings>('/api/wifi/ap/settings')
+    if (result.isErr()) {
+      console.error('Error:', result.inner)
+      return
+    }
+    apSettings = result.inner
+    return apSettings
+  }
+
+  const interval = setInterval(async () => {
+    getAPStatus()
+  }, 5000)
+
+  onDestroy(() => clearInterval(interval))
+
+  onMount(getAPSettings)
+
+  let provisionMode = [
+    {
+      id: 0,
+      text: `Always`
+    },
+    {
+      id: 1,
+      text: `When WiFi Disconnected`
+    },
+    {
+      id: 2,
+      text: `Never`
+    }
+  ]
+
+  type Variant = 'success' | 'error' | 'primary' | 'info' | 'warning'
+
+  let apStatusVariant: Variant[] = ['success', 'error', 'warning']
+
+  let apStatusDescription = ['Active', 'Inactive', 'Lingering']
+
+  let formErrors = $state({
+    ssid: false,
+    channel: false,
+    max_clients: false,
+    local_ip: false,
+    gateway_ip: false,
+    subnet_mask: false
+  })
+
+  async function postAPSettings(data: ApSettings) {
+    const result = await api.post<ApSettings>('/api/wifi/ap/settings', data)
+    if (result.isErr()) {
+      notifications.error('User not authorized.', 3000)
+      console.error('Error:', result.inner)
+      return
+    }
+    notifications.success('Access Point settings updated.', 3000)
+    apSettings = result.inner
+  }
+
+  function handleSubmitAP() {
+    let valid = true
+
+    // Validate SSID
+    if (apSettings.ssid.length < 3 || apSettings.ssid.length > 32) {
+      valid = false
+      formErrors.ssid = true
+    } else {
+      formErrors.ssid = false
     }
 
-    async function getAPSettings() {
-        const result = await api.get<ApSettings>('/api/wifi/ap/settings');
-        if (result.isErr()) {
-            console.error('Error:', result.inner);
-            return;
-        }
-        apSettings = result.inner;
-        return apSettings;
+    // Validate Channel
+    let channel = Number(apSettings.channel)
+    if (1 > channel || channel > 13) {
+      valid = false
+      formErrors.channel = true
+    } else {
+      formErrors.channel = false
     }
 
-    const interval = setInterval(async () => {
-        getAPStatus();
-    }, 5000);
-
-    onDestroy(() => clearInterval(interval));
-
-    onMount(getAPSettings);
-
-    let provisionMode = [
-        {
-            id: 0,
-            text: `Always`
-        },
-        {
-            id: 1,
-            text: `When WiFi Disconnected`
-        },
-        {
-            id: 2,
-            text: `Never`
-        }
-    ];
-
-    let apStatusDescription = [
-        { bg_color: 'bg-success', text_color: 'text-success-content', description: 'Active' },
-        { bg_color: 'bg-error', text_color: 'text-error-content', description: 'Inactive' },
-        { bg_color: 'bg-warning', text_color: 'text-warning-content', description: 'Lingering' }
-    ];
-
-    let formErrors = $state({
-        ssid: false,
-        channel: false,
-        max_clients: false,
-        local_ip: false,
-        gateway_ip: false,
-        subnet_mask: false
-    });
-
-    async function postAPSettings(data: ApSettings) {
-        const result = await api.post<ApSettings>('/api/wifi/ap/settings', data);
-        if (result.isErr()) {
-            notifications.error('User not authorized.', 3000);
-            console.error('Error:', result.inner);
-            return;
-        }
-        notifications.success('Access Point settings updated.', 3000);
-        apSettings = result.inner;
+    // Validate max_clients
+    let maxClients = Number(apSettings.max_clients)
+    if (1 > maxClients || maxClients > 8) {
+      valid = false
+      formErrors.max_clients = true
+    } else {
+      formErrors.max_clients = false
     }
 
-    function handleSubmitAP() {
-        let valid = true;
+    // RegEx for IPv4
+    const regexExp =
+      /\b(?:(?:2(?:[0-4][0-9]|5[0-5])|[0-1]?[0-9]?[0-9])\.){3}(?:(?:2([0-4][0-9]|5[0-5])|[0-1]?[0-9]?[0-9]))\b/
 
-        // Validate SSID
-        if (apSettings.ssid.length < 3 || apSettings.ssid.length > 32) {
-            valid = false;
-            formErrors.ssid = true;
-        } else {
-            formErrors.ssid = false;
-        }
-
-        // Validate Channel
-        let channel = Number(apSettings.channel);
-        if (1 > channel || channel > 13) {
-            valid = false;
-            formErrors.channel = true;
-        } else {
-            formErrors.channel = false;
-        }
-
-        // Validate max_clients
-        let maxClients = Number(apSettings.max_clients);
-        if (1 > maxClients || maxClients > 8) {
-            valid = false;
-            formErrors.max_clients = true;
-        } else {
-            formErrors.max_clients = false;
-        }
-
-        // RegEx for IPv4
-        const regexExp =
-            /\b(?:(?:2(?:[0-4][0-9]|5[0-5])|[0-1]?[0-9]?[0-9])\.){3}(?:(?:2([0-4][0-9]|5[0-5])|[0-1]?[0-9]?[0-9]))\b/;
-
-        // Validate gateway IP
-        if (!regexExp.test(apSettings.gateway_ip)) {
-            valid = false;
-            formErrors.gateway_ip = true;
-        } else {
-            formErrors.gateway_ip = false;
-        }
-
-        // Validate Subnet Mask
-        if (!regexExp.test(apSettings.subnet_mask)) {
-            valid = false;
-            formErrors.subnet_mask = true;
-        } else {
-            formErrors.subnet_mask = false;
-        }
-
-        // Validate local IP
-        if (!regexExp.test(apSettings.local_ip)) {
-            valid = false;
-            formErrors.local_ip = true;
-        } else {
-            formErrors.local_ip = false;
-        }
-
-        // Submit JSON to REST API
-        if (valid) {
-            postAPSettings(apSettings);
-        }
+    // Validate gateway IP
+    if (!regexExp.test(apSettings.gateway_ip)) {
+      valid = false
+      formErrors.gateway_ip = true
+    } else {
+      formErrors.gateway_ip = false
     }
+
+    // Validate Subnet Mask
+    if (!regexExp.test(apSettings.subnet_mask)) {
+      valid = false
+      formErrors.subnet_mask = true
+    } else {
+      formErrors.subnet_mask = false
+    }
+
+    // Validate local IP
+    if (!regexExp.test(apSettings.local_ip)) {
+      valid = false
+      formErrors.local_ip = true
+    } else {
+      formErrors.local_ip = false
+    }
+
+    // Submit JSON to REST API
+    if (valid) {
+      postAPSettings(apSettings)
+    }
+  }
 </script>
 
 <SettingsCard collapsible={false}>
-    {#snippet icon()}
-        <AP  class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
-    {/snippet}
-    {#snippet title()}
-        <span >Access Point</span>
-    {/snippet}
-    <div class="w-full overflow-x-auto">
-        {#await getAPStatus()}
-            <Spinner />
-        {:then nothing}
-            <div
-                class="flex w-full flex-col space-y-1"
-                transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-                <div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2">
-                    <div
-                        class="mask mask-hexagon h-auto w-10 {apStatusDescription[apStatus.status]
-                            .bg_color}"
-                    >
-                        <AP
-                            class="h-auto w-full scale-75 {apStatusDescription[apStatus.status]
-                                .text_color}"
-                        />
-                    </div>
-                    <div>
-                        <div class="font-bold">Status</div>
-                        <div class="text-sm opacity-75">
-                            {apStatusDescription[apStatus.status].description}
-                        </div>
-                    </div>
-                </div>
+  {#snippet icon()}
+    <AP class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
+  {/snippet}
+  {#snippet title()}
+    <span>Access Point</span>
+  {/snippet}
+  <div class="w-full overflow-x-auto">
+    {#await getAPStatus()}
+      <Spinner />
+    {:then nothing}
+      <div
+        class="flex w-full flex-col space-y-1"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <StatusItem
+          icon={AP}
+          title="Status"
+          variant={apStatusVariant[apStatus.status]}
+          description={apStatusDescription[apStatus.status]} />
 
-                <div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2">
-                    <div class="mask mask-hexagon bg-primary h-auto w-10">
-                        <Home class="text-primary-content h-auto w-full scale-75" />
-                    </div>
-                    <div>
-                        <div class="font-bold">IP Address</div>
-                        <div class="text-sm opacity-75">
-                            {apStatus.ip_address}
-                        </div>
-                    </div>
-                </div>
+        <StatusItem icon={Home} title="IP Address" description={apStatus.ip_address} />
 
-                <div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2">
-                    <div class="mask mask-hexagon bg-primary h-auto w-10">
-                        <MAC class="text-primary-content h-auto w-full scale-75" />
-                    </div>
-                    <div>
-                        <div class="font-bold">MAC Address</div>
-                        <div class="text-sm opacity-75">
-                            {apStatus.mac_address}
-                        </div>
-                    </div>
-                </div>
+        <StatusItem icon={MAC} title="MAC Address" description={apStatus.mac_address} />
 
-                <div class="rounded-box bg-base-100 flex items-center space-x-3 px-4 py-2">
-                    <div class="mask mask-hexagon bg-primary h-auto w-10">
-                        <Devices class="text-primary-content h-auto w-full scale-75" />
-                    </div>
-                    <div>
-                        <div class="font-bold">AP Clients</div>
-                        <div class="text-sm opacity-75">
-                            {apStatus.station_num}
-                        </div>
-                    </div>
-                </div>
-            </div>
-        {/await}
+        <StatusItem icon={Devices} title="AP Clients" description={apStatus.station_num} />
+      </div>
+    {/await}
+  </div>
+
+  <div class="bg-base-200 relative grid w-full max-w-2xl self-center overflow-hidden">
+    <div
+      class="min-h-16 flex w-full items-center justify-between space-x-3 p-0 text-xl font-medium">
+      Change AP Settings
     </div>
+    {#await getAPSettings()}
+      <Spinner />
+    {:then nothing}
+      <div
+        class="flex flex-col gap-2 p-0"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <form
+          class="grid w-full grid-cols-1 content-center gap-x-4 p-0s sm:grid-cols-2"
+          onsubmit={preventDefault(handleSubmitAP)}
+          novalidate
+          bind:this={formField}>
+          <div>
+            <label class="label" for="apmode">
+              <span class="label-text">Provide Access Point ...</span>
+            </label>
+            <select
+              class="select select-bordered w-full"
+              id="apmode"
+              bind:value={apSettings.provision_mode}>
+              {#each provisionMode as mode}
+                <option value={mode.id}>
+                  {mode.text}
+                </option>
+              {/each}
+            </select>
+          </div>
+          <div>
+            <label class="label" for="ssid">
+              <span class="label-text text-md">SSID</span>
+            </label>
+            <input
+              type="text"
+              class="input input-bordered invalid:border-error w-full invalid:border-2 {(
+                formErrors.ssid
+              ) ?
+                'border-error border-2'
+              : ''}"
+              bind:value={apSettings.ssid}
+              id="ssid"
+              min="2"
+              max="32"
+              required />
+            <label class="label" for="ssid">
+              <span class="label-text-alt text-error {formErrors.ssid ? '' : 'hidden'}"
+                >SSID must be between 2 and 32 characters long</span>
+            </label>
+          </div>
 
-    <div class="bg-base-200 relative grid w-full max-w-2xl self-center overflow-hidden">
-        <div
-            class="min-h-16 flex w-full items-center justify-between space-x-3 p-0 text-xl font-medium"
-        >
-            Change AP Settings
-        </div>
-        {#await getAPSettings()}
-            <Spinner />
-        {:then nothing}
-            <div
-                class="flex flex-col gap-2 p-0"
-                transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-                <form
-                    class="grid w-full grid-cols-1 content-center gap-x-4 p-0s sm:grid-cols-2"
-                    onsubmit={preventDefault(handleSubmitAP)}
-                    novalidate
-                    bind:this={formField}
-                >
-                    <div>
-                        <label class="label" for="apmode">
-                            <span class="label-text">Provide Access Point ...</span>
-                        </label>
-                        <select
-                            class="select select-bordered w-full"
-                            id="apmode"
-                            bind:value={apSettings.provision_mode}
-                        >
-                            {#each provisionMode as mode}
-                                <option value={mode.id}>
-                                    {mode.text}
-                                </option>
-                            {/each}
-                        </select>
-                    </div>
-                    <div>
-                        <label class="label" for="ssid">
-                            <span class="label-text text-md">SSID</span>
-                        </label>
-                        <input
-                            type="text"
-                            class="input input-bordered invalid:border-error w-full invalid:border-2 {(
-                                formErrors.ssid
-                            ) ?
-                                'border-error border-2'
-                            :   ''}"
-                            bind:value={apSettings.ssid}
-                            id="ssid"
-                            min="2"
-                            max="32"
-                            required
-                        />
-                        <label class="label" for="ssid">
-                            <span
-                                class="label-text-alt text-error {formErrors.ssid ? '' : 'hidden'}"
-                                >SSID must be between 2 and 32 characters long</span
-                            >
-                        </label>
-                    </div>
+          <div>
+            <label class="label" for="pwd">
+              <span class="label-text text-md">Password</span>
+            </label>
+            <PasswordInput bind:value={apSettings.password} id="pwd" />
+          </div>
+          <div>
+            <label class="label" for="channel">
+              <span class="label-text text-md">Preferred Channel</span>
+            </label>
+            <input
+              type="number"
+              min="1"
+              max="13"
+              class="input input-bordered invalid:border-error w-full invalid:border-2 {(
+                formErrors.channel
+              ) ?
+                'border-error border-2'
+              : ''}"
+              bind:value={apSettings.channel}
+              id="channel"
+              required />
+            <label class="label" for="channel">
+              <span class="label-text-alt text-error {formErrors.channel ? '' : 'hidden'}"
+                >Must be channel 1 to 13</span>
+            </label>
+          </div>
 
-                    <div>
-                        <label class="label" for="pwd">
-                            <span class="label-text text-md">Password</span>
-                        </label>
-                        <PasswordInput bind:value={apSettings.password} id="pwd" />
-                    </div>
-                    <div>
-                        <label class="label" for="channel">
-                            <span class="label-text text-md">Preferred Channel</span>
-                        </label>
-                        <input
-                            type="number"
-                            min="1"
-                            max="13"
-                            class="input input-bordered invalid:border-error w-full invalid:border-2 {(
-                                formErrors.channel
-                            ) ?
-                                'border-error border-2'
-                            :   ''}"
-                            bind:value={apSettings.channel}
-                            id="channel"
-                            required
-                        />
-                        <label class="label" for="channel">
-                            <span
-                                class="label-text-alt text-error {formErrors.channel ? '' : (
-                                    'hidden'
-                                )}">Must be channel 1 to 13</span
-                            >
-                        </label>
-                    </div>
+          <div>
+            <label class="label" for="clients">
+              <span class="label-text text-md">Max Clients</span>
+            </label>
+            <input
+              type="number"
+              min="1"
+              max="8"
+              class="input input-bordered invalid:border-error w-full invalid:border-2 {(
+                formErrors.max_clients
+              ) ?
+                'border-error border-2'
+              : ''}"
+              bind:value={apSettings.max_clients}
+              id="clients"
+              required />
+            <label class="label" for="clients">
+              <span class="label-text-alt text-error {formErrors.max_clients ? '' : 'hidden'}"
+                >Maximum 8 clients allowed</span>
+            </label>
+          </div>
 
-                    <div>
-                        <label class="label" for="clients">
-                            <span class="label-text text-md">Max Clients</span>
-                        </label>
-                        <input
-                            type="number"
-                            min="1"
-                            max="8"
-                            class="input input-bordered invalid:border-error w-full invalid:border-2 {(
-                                formErrors.max_clients
-                            ) ?
-                                'border-error border-2'
-                            :   ''}"
-                            bind:value={apSettings.max_clients}
-                            id="clients"
-                            required
-                        />
-                        <label class="label" for="clients">
-                            <span
-                                class="label-text-alt text-error {formErrors.max_clients ? '' : (
-                                    'hidden'
-                                )}">Maximum 8 clients allowed</span
-                            >
-                        </label>
-                    </div>
+          <div>
+            <label class="label" for="localIP">
+              <span class="label-text text-md">Local IP</span>
+            </label>
+            <input
+              type="text"
+              class="input input-bordered w-full {formErrors.local_ip ? 'border-error border-2' : (
+                ''
+              )}"
+              minlength="7"
+              maxlength="15"
+              size="15"
+              bind:value={apSettings.local_ip}
+              id="localIP"
+              required />
+            <label class="label" for="localIP">
+              <span class="label-text-alt text-error {formErrors.local_ip ? '' : 'hidden'}"
+                >Must be a valid IPv4 address</span>
+            </label>
+          </div>
 
-                    <div>
-                        <label class="label" for="localIP">
-                            <span class="label-text text-md">Local IP</span>
-                        </label>
-                        <input
-                            type="text"
-                            class="input input-bordered w-full {formErrors.local_ip ?
-                                'border-error border-2'
-                            :   ''}"
-                            minlength="7"
-                            maxlength="15"
-                            size="15"
-                            bind:value={apSettings.local_ip}
-                            id="localIP"
-                            required
-                        />
-                        <label class="label" for="localIP">
-                            <span
-                                class="label-text-alt text-error {formErrors.local_ip ? '' : (
-                                    'hidden'
-                                )}">Must be a valid IPv4 address</span
-                            >
-                        </label>
-                    </div>
+          <div>
+            <label class="label" for="gateway">
+              <span class="label-text text-md">Gateway IP</span>
+            </label>
+            <input
+              type="text"
+              class="input input-bordered w-full {formErrors.gateway_ip ? 'border-error border-2'
+              : ''}"
+              minlength="7"
+              maxlength="15"
+              size="15"
+              bind:value={apSettings.gateway_ip}
+              id="gateway"
+              required />
+            <label class="label" for="gateway">
+              <span class="label-text-alt text-error {formErrors.gateway_ip ? '' : 'hidden'}"
+                >Must be a valid IPv4 address</span>
+            </label>
+          </div>
+          <div>
+            <label class="label" for="subnet">
+              <span class="label-text text-md">Subnet Mask</span>
+            </label>
+            <input
+              type="text"
+              class="input input-bordered w-full {formErrors.subnet_mask ? 'border-error border-2'
+              : ''}"
+              minlength="7"
+              maxlength="15"
+              size="15"
+              bind:value={apSettings.subnet_mask}
+              id="subnet"
+              required />
+            <label class="label" for="subnet">
+              <span class="label-text-alt text-error {formErrors.subnet_mask ? '' : 'hidden'}"
+                >Must be a valid IPv4 address</span>
+            </label>
+          </div>
 
-                    <div>
-                        <label class="label" for="gateway">
-                            <span class="label-text text-md">Gateway IP</span>
-                        </label>
-                        <input
-                            type="text"
-                            class="input input-bordered w-full {formErrors.gateway_ip ?
-                                'border-error border-2'
-                            :   ''}"
-                            minlength="7"
-                            maxlength="15"
-                            size="15"
-                            bind:value={apSettings.gateway_ip}
-                            id="gateway"
-                            required
-                        />
-                        <label class="label" for="gateway">
-                            <span
-                                class="label-text-alt text-error {formErrors.gateway_ip ? '' : (
-                                    'hidden'
-                                )}">Must be a valid IPv4 address</span
-                            >
-                        </label>
-                    </div>
-                    <div>
-                        <label class="label" for="subnet">
-                            <span class="label-text text-md">Subnet Mask</span>
-                        </label>
-                        <input
-                            type="text"
-                            class="input input-bordered w-full {formErrors.subnet_mask ?
-                                'border-error border-2'
-                            :   ''}"
-                            minlength="7"
-                            maxlength="15"
-                            size="15"
-                            bind:value={apSettings.subnet_mask}
-                            id="subnet"
-                            required
-                        />
-                        <label class="label" for="subnet">
-                            <span
-                                class="label-text-alt text-error {formErrors.subnet_mask ? '' : (
-                                    'hidden'
-                                )}">Must be a valid IPv4 address</span
-                            >
-                        </label>
-                    </div>
+          <label class="label my-auto cursor-pointer justify-start gap-4">
+            <input
+              type="checkbox"
+              bind:checked={apSettings.ssid_hidden}
+              class="checkbox checkbox-primary" />
+            <span class="">Hide SSID</span>
+          </label>
 
-                    <label class="label my-auto cursor-pointer justify-start gap-4">
-                        <input
-                            type="checkbox"
-                            bind:checked={apSettings.ssid_hidden}
-                            class="checkbox checkbox-primary"
-                        />
-                        <span class="">Hide SSID</span>
-                    </label>
-
-                    <div class="place-self-end">
-                        <button class="btn btn-primary" type="submit">Apply Settings</button>
-                    </div>
-                </form>
-            </div>
-        {/await}
-    </div>
+          <div class="place-self-end">
+            <button class="btn btn-primary" type="submit">Apply Settings</button>
+          </div>
+        </form>
+      </div>
+    {/await}
+  </div>
 </SettingsCard>

app/src/routes/wifi/mdns/+page.svelte

@@ -0,0 +1,7 @@
+<script lang="ts">
+  import MDNS from './MDNS.svelte'
+</script>
+
+<div class="mx-0 my-1 flex flex-col space-y-4 sm:mx-8 sm:my-8">
+  <MDNS />
+</div>

app/src/routes/wifi/mdns/MDNS.svelte

@@ -0,0 +1,100 @@
+<script lang="ts">
+  import { onMount } from 'svelte'
+  import { api } from '$lib/api'
+  import SettingsCard from '$lib/components/SettingsCard.svelte'
+  import { AP, Home, MAC, Devices } from '$lib/components/icons'
+  import Spinner from '$lib/components/Spinner.svelte'
+  import StatusItem from '$lib/components/StatusItem.svelte'
+  import { cubicOut } from 'svelte/easing'
+  import { slide } from 'svelte/transition'
+  import type { MDNSStatus, MDNSServiceItem, MDNSServiceQuery } from '$lib/types/models'
+  import { compareIp } from '$lib/utilities'
+
+  let mdnsStatus: MDNSStatus | undefined = $state()
+  let services: MDNSServiceItem[] = $state([])
+  let isLoading = $state(false)
+
+  const getMDNSStatus = async () => {
+    const result = await api.get<MDNSStatus>('/api/mdns/status')
+    if (result.isErr()) {
+      console.error('Error:', result.inner)
+      return
+    }
+    mdnsStatus = result.inner
+  }
+
+  const queryMDNSServices = async () => {
+    isLoading = true
+    const result = await api.post<MDNSServiceQuery>('/api/mdns/query', {
+      service: 'http',
+      protocol: 'tcp'
+    })
+    if (result.isErr()) {
+      console.error('Error:', result.inner)
+      return
+    }
+    services = result.inner.services.sort((a, b) => compareIp(a.ip, b.ip))
+    isLoading = false
+  }
+
+  onMount(async () => {
+    await getMDNSStatus()
+    await queryMDNSServices()
+  })
+
+  const triggerScan = async () => {
+    await queryMDNSServices()
+  }
+</script>
+
+<SettingsCard collapsible={false}>
+  {#snippet icon()}
+    <AP class="lex-shrink-0 mr-2 h-6 w-6 self-end" />
+  {/snippet}
+  {#snippet title()}
+    <span>MDNS</span>
+  {/snippet}
+  {#snippet right()}
+    <button class="btn btn-primary" onclick={triggerScan} disabled={isLoading}>
+      {#if isLoading}
+        <span class="loading loading-ring loading-xs"></span>
+      {:else}
+        Scan
+      {/if}
+    </button>
+  {/snippet}
+  <div class="w-full overflow-x-auto">
+    {#if mdnsStatus}
+      <div
+        class="flex w-full flex-col space-y-1"
+        transition:slide|local={{ duration: 300, easing: cubicOut }}>
+        <StatusItem icon={Home} title="IP Address" description={mdnsStatus.hostname} />
+
+        <StatusItem icon={MAC} title="Instance" description={mdnsStatus.instance} />
+
+        <StatusItem icon={Devices} title="Services" description={mdnsStatus.services.length} />
+
+        <table class="table">
+          <thead>
+            <tr>
+              <th></th>
+              <th>Name</th>
+              <th>Ip address</th>
+              <th>Port</th>
+            </tr>
+          </thead>
+          <tbody>
+            {#each services as service}
+              <tr>
+                <td><Devices class="h-6 w-6" /></td>
+                <td>{service.name}</td>
+                <td>{service.ip}</td>
+                <td>{service.port}</td>
+              </tr>
+            {/each}
+          </tbody>
+        </table>
+      </div>
+    {/if}
+  </div>
+</SettingsCard>

docs/1_components.md

@@ -1,17 +1,17 @@
 # Components
 
-Spot is comprised of a 3D printed body, some hardware and list of electronic components.
+Spot is comprised of a 3D-printed body, some hardware, and a list of electronic components.
 
 ## Hardware
 
-Spot is 3D printed and is a combination of different Spot Micro designs, with some minor modification on top.
-The original design is developed by KDY0523.
+Spot is 3D-printed and is a combination of different Spot Micro designs, with some minor modifications.
+The original design was developed by KDY0523.
 
 - [robjk reinforced shoulder remix](https://www.thingiverse.com/thing:4937631)
 - [Kooba SpotMicroESP32 remix](https://www.thingiverse.com/thing:4559827)
 - [KDY0532 original design](https://www.thingiverse.com/thing:3445283)
 
-The 3D prints is assembled with some additional component:
+The 3D prints are assembled with some additional non-printable components:
 
 - 84x M2x8 screws + M2 nuts
 - 92x M3x8 screws + M3 nuts
@@ -20,7 +20,7 @@ The 3D prints is assembled with some additional component:
 
 ## Electronics
 
-These are the electronics i used for mine and can easily be switched up to suit your Spot's needs.
+These are the electronics I used for mine, and they can easily be swapped to suit your Spot's needs.
 
 | Component                 | Specification                 | Required | Recommendation                                                                                          |
 | ------------------------- | ----------------------------- | -------- | ------------------------------------------------------------------------------------------------------- |
@@ -39,6 +39,6 @@ These are the electronics i used for mine and can easily be switched up to suit
 | 7.6-8.4V Battery          | Battery                       | No       | Im using 4x 18650 in 2s2p configuration, but other people have 2s LiPos.                                |
 | 4x Servo extension cables | Servo extension cables        | Yes      | You can either buy them or make them with a couple or headers and some cable.                           |
 
-I recommend getting a ESP32-S3 with a camera, allowing for more computation and imaging capabilities.
+I recommend getting an ESP32-S3 with a camera, allowing for more computation and imaging capabilities.
 
-It means a more responsive robot as its faster doing sensor fusion, calculating kinematic and gait planning, and networking.
+It means a more responsive robot as it's faster at doing sensor fusion, calculating kinematics and gait planning, and networking.

docs/2_assembly.md

@@ -1,6 +1,6 @@
 # Assembly and calibration
 
-There exist a number of great resources for the assembly of the spot micro. For this reason I refer to these, as the steps are the same for this version:
+There are a number of great resources for the assembly of the Spot Micro. For this reason, I refer to these, as the steps are the same for this version:
 
 - [Michael Kubina SpotMicroESP32 assembly](https://github.com/michaelkubina/SpotMicroESP32/tree/master/assembly)
 - [Spot Micro AI assembly](https://spotmicroai.readthedocs.io/en/latest/assembly/)
@@ -9,7 +9,7 @@ There exist a number of great resources for the assembly of the spot micro. For
 
 Discussion about [Calibration](https://github.com/runeharlyk/SpotMicroESP32-Leika/discussions/118)
 
-Assuming the servos are connected to the PCA9685 and is powered on:
+Assuming the servos are connected to the PCA9685 and are powered on:
 
 ### Calibrate in servo frame
 
@@ -38,13 +38,28 @@ You now have the values for the servos.
 
 ### Calibration in body frame
 
-They now has to calibrated to the body frame. It assumed they have the center pwm pointing straight down.
+They now have to be calibrated to the body frame. It is assumed they have the center PWM pointing straight down.
 
 1. Navigate to `/controller` and click on "Calibrate". This will set the servo to the center pwm value.
 2. Navigate to `peripherals/servo` - Here you can set the servo angle offset.
 
-All the legs should be pointing down. If they are not you have to options. 1; Physically move the servos to the correct position by un screwing the servo horns. 2; Update the servo offset in the servo table.
+All the legs should be pointing down. If they are not, you have two options. 1. Physically move the servos to the correct position by unscrewing the servo horns. 2. Update the servo offset in the servo table.
 
 ## Circuit diagram
 
-![Electronics diagram](media/circuit.png "Title")
+![Electronics diagram](media/circuitschematic.png "Title")
+
+PCA9685 Servo PWM numbers to joint:
+| PWM_0  | Front Left Shoulder          |
+|--------|------------------------------|
+| PWM_1  | Front Left Upper-Limb        |
+| PWM_2  | Front Left Leg (Lower-Limb)  |
+| PWM_3  | Front Right Shoulder         |
+| PWM_4  | Front Right Upper-Limb       |
+| PWM_5  | Front Right Leg (Lower-Limb) |
+| PWM_6  | Rear Left Shoulder           |
+| PWM_7  | Rear Left Upper-Limb         |
+| PWM_8  | Rear Left Leg (Lower-Limb)   |
+| PWM_9  | Rear Right Shoulder          |
+| PWM_10 | Rear Right Upper-Limb        |
+| PWM_11 | Rear Right Leg (Lower-limb)  |

docs/3_software.md

@@ -1,6 +1,6 @@
 # Software
 
-The robots firmware is built using platform io using the arduino framework over ESP-IDF.
+The robot's firmware is built using PlatformIO with the Arduino framework over ESP-IDF.
 
 ## Prerequisites
 
@@ -8,7 +8,7 @@ To prepare the frontend code for the ESP32, a specific build chain is required.
 
 ### Required Software
 
-Install the following software to ensure all functionalities:
+Install the following software to ensure all functionality:
 
 - [VSCode](https://code.visualstudio.com/) - Preferred IDE for development
 - [Node.js](https://nodejs.org) - Needed for app building
@@ -45,9 +45,9 @@ For additional boards, refer to the [official board list](https://docs.platformi
 
 ### Factory settings
 
-Update the `esp32/factory_setting.ini` with new wifi settings, app name and other device information.
+Update the `esp32/factory_setting.ini` with new Wi-Fi settings, app name and other device information.
 
 ### Build & Upload Process
 
-Update the `platformio.ini` file for your board, then navigate to the PlatformIO tab, select your environment, click `Upload Filesystem Image` and after uploading finish, click `Upload and Monitor`. The filesystem image only has to be uploaded the first time and will override config files on the microcontroller.
-When uploading new firmware the app is evaluated and if necessary will be rebuild.
+Update the `platformio.ini` file for your board, then navigate to the PlatformIO tab, select your environment, click `Upload Filesystem Image` and after uploading finishes, click `Upload and Monitor`. The filesystem image only needs to be uploaded the first time. It will override config files on the microcontroller.
+When uploading new firmware, the app is evaluated, and if necessary, will be rebuilt.

docs/4_configuring.md

@@ -4,10 +4,10 @@
 
 ## Connecting to the network
 
-If the wifi settings were configured using `esp32/factory_settings.ini` the robot will try to connect to the network.
+If the Wi-Fi settings were configured using `esp32/factory_settings.ini`, the robot will try to connect to the network.
 
-If it fails to connect, it will host a AP with a captive portal where it's possible to configure wifi settings.
+If it fails to connect, it will host an AP with a captive portal where it's possible to configure Wi-Fi settings.
 
-When the robot connect successfully the ip address will be printed to the serial monitor
+When the robot connects successfully, the IP address will be printed to the serial monitor.
 
 <!-- ## Calibrating servos -->

docs/5_running.md

@@ -1,10 +1,10 @@
 # Running the spot
 
-> *Prerequsition*: You have successfully build, flashed and configured your robot.
+> *Prerequsition*: You have successfully built, flashed, and configured your robot.
 
 Navigate to `/controller`
 
 ![Controller](media/controller.png)
 
-<!-- When the robot is in a safe position, click on rest.
+<!-- When the robot is in a safe position, click on Rest.
 This will activate the servos and put the robot in the rest position. -->

docs/6_developing.md

@@ -1,12 +1,12 @@
 # Developing
 
-> _Prerequsition_: You have successfully build, flashed and configured your robot.
+> _Prerequsition_: You have successfully built, flashed, and configured your robot.
 
 ## Setting up SvelteKit
 
 ### Proxy Configuration for Development
 
-Configure the proxy settings in the `vite.config.ts` file to direct API calls to your ESP32 device. By default it used the factory MDNS address, but can be changed to the ip if preferred.
+Configure the proxy settings in the `vite.config.ts` file to direct API calls to your ESP32 device. By default, it uses the factory MDNS address, but it can be changed to the IP if preferred.
 
 ```ts
 server: {

docs/motion_system.md

@@ -1,10 +1,10 @@
 # 🏁 Motion state controller
 
-The motion controller is a finite state machine with state allowing for static and dynamic posing, 8-phase crawl and bezier bases trot gait, and choreographed animation.
+The motion controller is a finite state machine that allows for static and dynamic posing, 8-phase crawl, bezier-based trot gait, and choreographed animation.
 
 ## Controller Input Mapping
 
-The controller input is interpret different between the modes. For the walking it it looks like this:
+The controller input is interpreted differently between the modes. For walking, it looks like this:
 
 | Controller Input | Mapped to Gait Step | Range   |
 | ---------------- | ------------------- | ------- |
@@ -21,7 +21,7 @@ The controller input is interpret different between the modes. For the walking i
 
 ## Walking gait
 
-General about walking gait
+General description of walking gait.
 
 Time step
 
@@ -31,9 +31,9 @@ Stance and swing controller
 
 ## 8-phase crawl gait
 
-The 8-phase crawl gait works by lifting one leg at a time while shifting its body weight away from the leg.
+The 8-phase crawl gait works by lifting one leg at a time while shifting the body weight away from that leg.
 
-As the name implies, the gait consist of 8 discrete phases, which represents which feet should be contact the ground or be in swing.
+As the name implies, the gait consists of 8 discrete phases, which represent which feet should be in contact with the ground or in swing.
 
 At each time step the phase time $t\in [0,1]$ is updated. When $t\geq 1$ the phase index is updated and phase time is reset.
 

esp32/.gitignore

@@ -2,6 +2,6 @@
 data/
 www/
 build/
-lib/ESP32-sveltekit/WWWData.h
+include/WWWData.h
 **/.vscode/c_cpp_properties.json
 **/.vscode/launch.json

esp32/build_settings.ini

@@ -5,4 +5,6 @@ build_flags =
     -D EMBED_WWW
     -D SERVE_CONFIG_FILES
     -D CORS_ORIGIN=\"*\"
-    -D ENABLE_CORS
+    -D ENABLE_CORS
+    -D USE_MSGPACK=1 ; Use either msgpack or json
+    -D USE_JSON=0    ; Use either msgpack or json

esp32/factory_settings.ini

@@ -7,7 +7,7 @@
 [factory_settings]
 build_flags =
     -D APP_NAME=\"Spot-Micro\" ; [a-zA-Z0-9-_]
-    -D APP_VERSION=\"0.0.1\"
+    -D APP_VERSION=\"0.1.0\"
 
     ; WiFi settings
     -D FACTORY_WIFI_SSID=\"\"

esp32/features.ini

@@ -1,15 +1,21 @@
 [features]
 build_flags = 
-  -D USE_SLEEP=0
-  -D USE_UPLOAD_FIRMWARE=1
+  ; Kinematics - Choose only one
+  -D SPOTMICRO_ESP32
+  ;-D SPOTMICRO_YERTLE
+
+  ; Firmware flags
+  -D USE_SLEEP=1
+  -D USE_UPLOAD_FIRMWARE=0
   -D USE_DOWNLOAD_FIRMWARE=0
   -D USE_MOTION=1
+  -D USE_MDNS=1
 
   ; Hardware specific
-  -D USE_IMU=0
-  -D USE_MAG=0
-  -D USE_BMP=0
-  -D USE_GPS=0
+  -D USE_HMC5883=0
+  -D USE_BMP180=0
+  -D USE_MPU6050=0
   -D USE_WS2812=1
+  -D USE_BNO055=0
   -D USE_USS=0
-  -D USE_SERVO=1
+  -D USE_PCA9685=1

esp32/include/adapters/bluetooth.hpp

@@ -0,0 +1,43 @@
+#pragma once
+#include <BLEDevice.h>
+#include <BLEServer.h>
+#include <BLEUtils.h>
+#include <BLE2902.h>
+
+#include "comm_base.hpp"
+#include "event_bus.hpp"
+#include "topic.hpp"
+
+class BluetoothService : public CommBase<> {
+    BLEServer* bleServer {nullptr};
+    BLECharacteristic* txCharacteristic {nullptr};
+    BLECharacteristic* rxCharacteristic {nullptr};
+    bool connected {false};
+
+  public:
+    void begin(const char* name);
+
+  private:
+    void handleReceive(const std::string& data);
+    void send(size_t clientId, const char* data, size_t len) override;
+
+    struct ServerCb : BLEServerCallbacks {
+        BluetoothService* svc;
+        ServerCb(BluetoothService* s) : svc(s) {}
+        void onConnect(BLEServer*) override { svc->connected = true; }
+        void onDisconnect(BLEServer* s) override {
+            svc->connected = false;
+            for (size_t i = 0; i < static_cast<size_t>(Topic::COUNT); ++i) svc->unsubscribe(static_cast<Topic>(i), 0);
+            svc->bleServer->startAdvertising();
+        }
+    };
+
+    struct RxCb : BLECharacteristicCallbacks {
+        BluetoothService* svc;
+        RxCb(BluetoothService* s) : svc(s) {}
+        void onWrite(BLECharacteristic* c) override {
+            auto v = c->getValue();
+            if (!v.empty()) svc->handleReceive(v);
+        }
+    };
+};

esp32/include/adapters/comm_base.hpp

@@ -0,0 +1,95 @@
+#pragma once
+#include "event_bus.hpp"
+#include <ArduinoJson.h>
+#include <array>
+#include <bitset>
+#include <freertos/FreeRTOS.h>
+#include "topic.hpp"
+
+#ifndef MAX_CID
+#define MAX_CID 64
+#endif
+
+enum class MsgKind : uint8_t { Connect = 0, Disconnect = 1, Event = 2, Ping = 3, Pong = 4 };
+
+template <size_t MaxCid = MAX_CID, size_t NTopics = static_cast<size_t>(Topic::COUNT)>
+class CommBase {
+    using Bits = std::bitset<MaxCid>;
+    std::array<Bits, NTopics> subs_;
+    portMUX_TYPE mux_ portMUX_INITIALIZER_UNLOCKED;
+
+    std::array<void*, NTopics> subscriptionHandle {};
+
+    static constexpr size_t invalid = SIZE_MAX;
+
+    static constexpr size_t idx(Topic t) {
+        size_t i = static_cast<size_t>(t);
+        return i < NTopics ? i : invalid;
+    }
+
+    template <Topic T>
+    void encode(JsonDocument& d, const typename TopicTraits<T>::Msg& m) {
+        auto a = d.to<JsonArray>();
+        a.add(static_cast<uint8_t>(MsgKind::Event));
+        a.add(static_cast<uint8_t>(T));
+        toJson(a.add<JsonVariant>(), m);
+    }
+
+  protected:
+    virtual void send(size_t cid, const char* data, size_t len) = 0;
+
+    template <class Msg>
+    auto& getHandle(Topic topic) {
+        using H = typename EventBus<Msg>::Handle;
+        static H dummy;
+        auto* p = static_cast<H*>(subscriptionHandle[size_t(topic)]);
+        return p ? *p : dummy;
+    }
+
+    template <class Msg>
+    void setHandle(Topic topic, typename EventBus<Msg>::Handle&& h) {
+        using H = typename EventBus<Msg>::Handle;
+        subscriptionHandle[size_t(topic)] = new H(std::move(h));
+    }
+
+  public:
+    void subscribe(Topic t, size_t cid) {
+        size_t i = idx(t);
+        if (i == invalid) return;
+        portENTER_CRITICAL(&mux_);
+        subs_[i].set(cid);
+        portEXIT_CRITICAL(&mux_);
+    }
+
+    void unsubscribe(Topic t, size_t cid) {
+        size_t i = idx(t);
+        if (i == invalid) return;
+        portENTER_CRITICAL(&mux_);
+        subs_[i].reset(cid);
+        portEXIT_CRITICAL(&mux_);
+    }
+
+    bool has(Topic t) const {
+        size_t i = idx(t);
+        return i == invalid ? false : subs_[i].any();
+    }
+
+    template <Topic T>
+    void emit(const typename TopicTraits<T>::Msg& m) {
+        constexpr size_t i = idx(T);
+        if (i == invalid) return;
+        if (!subs_[i].any()) return;
+
+        JsonDocument doc;
+        encode<T>(doc, m);
+        String out;
+#if USE_MSGPACK
+        serializeMsgPack(doc, out);
+#else
+        serializeJson(doc, out);
+#endif
+        auto& b = subs_[i];
+        for (size_t cid = 0; cid < MaxCid; ++cid)
+            if (b.test(cid)) send(cid, out.c_str(), out.length());
+    }
+};

esp32/include/adapters/websocket.hpp

@@ -0,0 +1,33 @@
+#ifndef Socket_h
+#define Socket_h
+
+#include <PsychicHttp.h>
+#include <ArduinoJson.h>
+#include <map>
+#include <list>
+#include <functional>
+
+#include "event_bus.hpp"
+#include "adapters/comm_base.hpp"
+#include "topic.hpp"
+
+class EventSocket : public CommBase<> {
+    PsychicWebSocketHandler _socket;
+
+  public:
+    EventSocket();
+    PsychicWebSocketHandler *getHandler() { return &_socket; }
+
+  private:
+    void send(size_t clientId, const char *data, size_t len) override;
+
+    void handleReceive(const std::string &data);
+
+    void onWSOpen(PsychicWebSocketClient *client);
+    void onWSClose(PsychicWebSocketClient *client);
+    esp_err_t onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *frame);
+};
+
+extern EventSocket socket;
+
+#endif

esp32/include/event_bus.hpp

@@ -0,0 +1,259 @@
+#pragma once
+#include <array>
+#include <optional>
+#include <atomic>
+#include <cstddef>
+#include <cstring>
+#include <type_traits>
+#include <utility>
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+#include <freertos/queue.h>
+
+template <typename Sig, size_t MaxSize>
+class FixedFn;
+
+template <typename R, typename... A, size_t MaxSize>
+class FixedFn<R(A...), MaxSize> {
+    alignas(void*) std::byte buf[MaxSize];
+    void (*call)(void*, A&&...) {};
+    void (*moveFn)(void*, void*) {};
+    void (*destroy)(void*) {};
+
+  public:
+    template <typename Fun>
+    void set(Fun&& f) {
+        static_assert(sizeof(Fun) <= MaxSize);
+        new (buf) Fun(std::forward<Fun>(f));
+        call = [](void* p, A&&... as) { (*reinterpret_cast<Fun*>(p))(std::forward<A>(as)...); };
+        moveFn = [](void* d, void* s) { new (d) Fun(std::move(*reinterpret_cast<Fun*>(s))); };
+        destroy = [](void* p) { reinterpret_cast<Fun*>(p)->~Fun(); };
+    }
+    R operator()(A... as) const {
+        return call(const_cast<void*>(static_cast<const void*>(buf)), std::forward<A>(as)...);
+    }
+    FixedFn() = default;
+    FixedFn(FixedFn&& o) {
+        if (o.moveFn) o.moveFn(buf, o.buf);
+        call = o.call;
+        moveFn = o.moveFn;
+        destroy = o.destroy;
+        o.destroy = nullptr;
+    }
+    FixedFn(const FixedFn& o) {
+        std::memcpy(buf, o.buf, MaxSize);
+        call = o.call;
+        moveFn = o.moveFn;
+        destroy = o.destroy;
+    }
+    ~FixedFn() {
+        if (destroy) destroy(buf);
+    }
+    FixedFn& operator=(const FixedFn&) = delete;
+    FixedFn& operator=(FixedFn&&) = delete;
+};
+
+enum class EmitMode { Latest, Batch };
+
+template <typename Msg, size_t QueueDepth = 64, size_t MaxSubs = 8, size_t BatchSize = 16>
+class EventBus {
+    struct Item {
+        Msg payload;
+        size_t exclude;
+    };
+    static constexpr size_t NO_EX = MaxSubs;
+    struct Sub {
+        FixedFn<void(const Msg*, size_t), 48> cb;
+        TickType_t interval;
+        TickType_t last;
+        EmitMode mode;
+        std::array<Msg, BatchSize> buf;
+        size_t cnt;
+    };
+    inline static StaticQueue_t qbuf;
+    inline static Item qStorage[QueueDepth];
+    inline static QueueHandle_t queue =
+        xQueueCreateStatic(QueueDepth, sizeof(Item), reinterpret_cast<uint8_t*>(qStorage), &qbuf);
+    inline static std::array<std::optional<Sub>, MaxSubs> subs {};
+    inline static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
+    inline static Msg latest {};
+    inline static std::atomic<bool> hasLatest {false};
+    inline static std::atomic<size_t> subCount {0};
+
+    static void store(const Msg& m) {
+        portENTER_CRITICAL(&mux);
+        latest = m;
+        hasLatest.store(true, std::memory_order_release);
+        portEXIT_CRITICAL(&mux);
+    }
+    static void storeISR(const Msg& m) {
+        UBaseType_t s = portSET_INTERRUPT_MASK_FROM_ISR();
+        latest = m;
+        hasLatest.store(true, std::memory_order_release);
+        portCLEAR_INTERRUPT_MASK_FROM_ISR(s);
+    }
+
+    static void dispatch(const Msg& m, size_t ex) {
+        TickType_t now = xTaskGetTickCount();
+        Sub* ready[MaxSubs];
+        size_t readyCnt = 0;
+
+        portENTER_CRITICAL(&mux);
+        for (size_t i = 0; i < MaxSubs; ++i) {
+            auto& opt = subs[i];
+            if (!opt || i == ex) continue;
+            Sub& s = *opt;
+            TickType_t dt = now - s.last;
+
+            if (s.interval && dt < s.interval) {
+                if (s.mode == EmitMode::Batch && s.cnt < BatchSize)
+                    s.buf[s.cnt++] = m;
+                else if (s.mode == EmitMode::Latest) {
+                    s.buf[0] = m;
+                    s.cnt = 1;
+                }
+            } else {
+                s.buf[s.cnt++] = m;
+                s.last = now;
+                ready[readyCnt++] = &s;
+            }
+        }
+        portEXIT_CRITICAL(&mux);
+
+        for (size_t i = 0; i < readyCnt; ++i) {
+            Sub* s = ready[i];
+            s->cb(s->buf.data(), s->cnt);
+            s->cnt = 0;
+        }
+    }
+
+    static void worker(void*) {
+        Item it;
+        while (xQueueReceive(queue, &it, portMAX_DELAY) == pdTRUE) dispatch(it.payload, it.exclude);
+    }
+    static void ensureTask() {
+        static bool once = (xTaskCreatePinnedToCore(worker, "evtbus", 4096, nullptr, 6, nullptr, 1), true);
+        (void)once;
+    }
+    static bool push(const Msg& m, size_t ex = NO_EX, TickType_t to = 0) {
+        ensureTask();
+        Item it {m, ex};
+        return xQueueSend(queue, &it, to) == pdTRUE;
+    }
+
+  public:
+    class Handle {
+        size_t idx {NO_EX};
+        friend class EventBus;
+        explicit Handle(size_t i) : idx(i) {}
+
+      public:
+        Handle() = default;
+        Handle(const Handle&) = delete;
+        Handle& operator=(const Handle&) = delete;
+        Handle(Handle&& o) noexcept : idx(o.idx) { o.idx = NO_EX; }
+        Handle& operator=(Handle&& o) noexcept {
+            if (this != &o) {
+                unsubscribe();
+                idx = o.idx;
+                o.idx = NO_EX;
+            }
+            return *this;
+        }
+        ~Handle() { unsubscribe(); }
+        void unsubscribe() {
+            if (idx < MaxSubs) {
+                portENTER_CRITICAL(&mux);
+                subs[idx].reset();
+                portEXIT_CRITICAL(&mux);
+                subCount.fetch_sub(1, std::memory_order_acq_rel);
+                idx = NO_EX;
+            }
+        }
+        bool valid() const { return idx < MaxSubs; }
+    };
+
+    template <typename C>
+    static Handle subscribe(uint32_t ms, EmitMode mode, C fn) {
+        ensureTask();
+        portENTER_CRITICAL(&mux);
+        for (size_t i = 0; i < MaxSubs; ++i)
+            if (!subs[i]) {
+                subs[i].emplace();
+                Sub& s = *subs[i];
+                s.cb.set(std::move(fn));
+                s.interval = pdMS_TO_TICKS(ms);
+                s.last = xTaskGetTickCount();
+                s.mode = mode;
+                s.cnt = 0;
+                subCount.fetch_add(1, std::memory_order_acq_rel);
+                portEXIT_CRITICAL(&mux);
+                return Handle(i);
+            }
+        portEXIT_CRITICAL(&mux);
+        return Handle(NO_EX);
+    }
+
+    template <typename C>
+    static Handle subscribe(C fn) {
+        if constexpr (std::is_invocable_v<C, const Msg*, size_t>)
+            return subscribe(0, EmitMode::Latest, std::move(fn));
+        else
+            return subscribe(0, EmitMode::Latest, [fn = std::move(fn)](const Msg* p, size_t n) {
+                for (size_t i = 0; i < n; ++i) fn(p[i]);
+            });
+    }
+
+    template <typename C>
+    static Handle subscribe(uint32_t ms, C fn) {
+        if constexpr (std::is_invocable_v<C, const Msg*, size_t>)
+            return subscribe(ms, EmitMode::Batch, std::move(fn));
+        else
+            return subscribe(ms, EmitMode::Batch, [fn = std::move(fn)](const Msg* p, size_t n) {
+                for (size_t i = 0; i < n; ++i) fn(p[i]);
+            });
+    }
+
+    static void publish(const Msg& m) {
+        store(m);
+        push(m, NO_EX, portMAX_DELAY);
+    }
+    static void publish(const Msg& m, const Handle& h) {
+        if (h.valid())
+            dispatch(m, h.idx);
+        else
+            publish(m);
+    }
+
+    static bool publishAsync(const Msg& m) {
+        store(m);
+        return push(m);
+    }
+    static bool publishAsync(const Msg& m, const Handle& h) {
+        if (h.valid()) dispatch(m, h.idx);
+        return publishAsync(m);
+    }
+
+    static void publishISR(const Msg& m, BaseType_t* hpw = nullptr) {
+        storeISR(m);
+        Item it {m, NO_EX};
+        xQueueSendFromISR(queue, &it, hpw);
+    }
+
+    static bool peek(Msg& out) {
+        if (!hasLatest.load(std::memory_order_acquire)) return false;
+        portENTER_CRITICAL(&mux);
+        out = latest;
+        portEXIT_CRITICAL(&mux);
+        return true;
+    }
+    static bool take(Msg& out) {
+        if (!hasLatest.load(std::memory_order_acquire)) return false;
+        portENTER_CRITICAL(&mux);
+        out = latest;
+        hasLatest.store(false, std::memory_order_release);
+        portEXIT_CRITICAL(&mux);
+        return true;
+    }
+    static bool hasSubscribers() { return subCount.load(std::memory_order_acquire) > 0; }
+};

esp32/include/features.h

@@ -28,18 +28,23 @@
 #endif
 
 // ESP32 IMU on by default
-#ifndef USE_IMU
-#define USE_IMU 1
+#ifndef USE_MPU6050
+#define USE_MPU6050 0
+#endif
+
+// ESP32 IMU on by default
+#ifndef USE_BNO055
+#define USE_BNO055 1
 #endif
 
 // ESP32 magnetometer on by default
-#ifndef USE_MAG
-#define USE_MAG 0
+#ifndef USE_HMC5883
+#define USE_HMC5883 0
 #endif
 
 // ESP32 barometer off by default
-#ifndef USE_BMP
-#define USE_BMP 0
+#ifndef USE_BMP180
+#define USE_BMP180 0
 #endif
 
 // ESP32 SONAR off by default
@@ -47,13 +52,37 @@
 #define USE_USS 0
 #endif
 
+// PCA9685 Servo controller on by default
+#ifndef USE_PCA9685
+#define USE_PCA9685 1
+#endif
+
 // ESP32 GPS off by default
 #ifndef USE_GPS
 #define USE_GPS 0
 #endif
 
+// ESP32 MDNS on by default
+#ifndef USE_MDNS
+#define USE_MDNS 1
+#endif
+
+// ESP32 MSGPACK on by default
+#ifndef USE_MSGPACK
+#define USE_MSGPACK 1
+#endif
+
+// ESP32 JSON off by default
+#ifndef USE_JSON
+#define USE_JSON 0
+#endif
+
+static_assert(!(USE_JSON == 1 && USE_MSGPACK == 1), "Cannot set both USE_JSON and USE_MSGPACK to 1 simultaneously");
+
 namespace feature_service {
 
+void printFeatureConfiguration();
+
 void features(JsonObject &root);
 
 esp_err_t getFeatures(PsychicRequest *request);

esp32/include/filesystem.h

@@ -12,6 +12,7 @@
 #define DEVICE_CONFIG_FILE "/config/peripheral.json"
 #define WIFI_SETTINGS_FILE "/config/wifiSettings.json"
 #define SERVO_SETTINGS_FILE "/config/servoSettings.json"
+#define MDNS_SETTINGS_FILE "/config/mdnsSettings.json"
 
 namespace FileSystem {
 extern PsychicUploadHandler *uploadHandler;
@@ -25,4 +26,6 @@ esp_err_t uploadFile(PsychicRequest *request, const String &filename, uint64_t i
 esp_err_t getFiles(PsychicRequest *request);
 esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json);
 esp_err_t handleEdit(PsychicRequest *request, JsonVariant &json);
+
+esp_err_t mkdir(PsychicRequest *request, JsonVariant &json);
 } // namespace FileSystem

esp32/include/firmware_download_service.h

@@ -4,7 +4,7 @@
 
 #include <WiFi.h>
 #include <ArduinoJson.h>
-#include <event_socket.h>
+#include <event_bus.hpp>
 #include <PsychicHttp.h>
 
 #include <HTTPClient.h>

esp32/include/firmware_upload_service.h

@@ -8,7 +8,7 @@
 
 #include <PsychicHttp.h>
 #include <system_service.h>
-#include <event_socket.h>
+#include <event_bus.hpp>
 
 enum FileType { ft_none = 0, ft_firmware = 1, ft_md5 = 2 };
 

esp32/include/kinematics.h

@@ -0,0 +1,167 @@
+#ifndef Kinematics_h
+#define Kinematics_h
+
+#include <utils/math_utils.h>
+
+struct alignas(16) body_state_t {
+    float omega, phi, psi, xm, ym, zm;
+    float feet[4][4];
+
+    void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
+
+    bool operator==(const body_state_t &other) const {
+        if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
+            !IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||
+            !IS_ALMOST_EQUAL(zm, other.zm)) {
+            return false;
+        }
+        return arrayEqual(feet, other.feet, 0.1f);
+    }
+};
+
+class Kinematics {
+  private:
+#if defined(SPOTMICRO_ESP32)
+    static constexpr float l1 = 60.5f / 100.0f;
+    static constexpr float l2 = 10.0f / 100.0f;
+    static constexpr float l3 = 111.2f / 100.0f;
+    static constexpr float l4 = 118.5f / 100.0f;
+
+    static constexpr float L = 207.5f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
+#elif defined(SPOTMICRO_YERTLE)
+    static constexpr float l1 = 35.0f / 100.0f;
+    static constexpr float l2 = 0.0f;
+    static constexpr float l3 = 130.0f / 100.0f;
+    static constexpr float l4 = 130.0f / 100.0f;
+
+    static constexpr float L = 240.0f / 100.0f;
+    static constexpr float W = 78.0f / 100.0f;
+#else
+#error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE"
+#endif
+
+    static constexpr float mountOffsets[4][3] = {
+        {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}};
+
+    static constexpr float invMountRot[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
+
+    alignas(16) float rot[3][3] = {0};
+    alignas(16) float inv_rot[3][3] = {0};
+    alignas(16) float inv_trans[3] = {0};
+
+    body_state_t currentState;
+
+  public:
+    static constexpr float default_feet_positions[4][4] = {
+        {mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1},
+        {mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1},
+        {mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1},
+        {mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 1},
+    };
+
+    esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
+        esp_err_t ret = ESP_OK;
+
+        if (currentState == body_state) return ESP_OK;
+
+        currentState.omega = body_state.omega;
+        currentState.phi = body_state.phi;
+        currentState.psi = body_state.psi;
+        currentState.xm = body_state.xm;
+        currentState.ym = body_state.ym;
+        currentState.zm = body_state.zm;
+        currentState.updateFeet(body_state.feet);
+
+        float roll = body_state.omega * DEG2RAD_F;
+        float pitch = body_state.phi * DEG2RAD_F;
+        float yaw = body_state.psi * DEG2RAD_F;
+        euler2R(roll, pitch, yaw, rot);
+        inverse(rot, inv_rot);
+
+        inv_trans[0] =
+            -inv_rot[0][0] * currentState.xm - inv_rot[0][1] * currentState.ym - inv_rot[0][2] * currentState.zm;
+        inv_trans[1] =
+            -inv_rot[1][0] * currentState.xm - inv_rot[1][1] * currentState.ym - inv_rot[1][2] * currentState.zm;
+        inv_trans[2] =
+            -inv_rot[2][0] * currentState.xm - inv_rot[2][1] * currentState.ym - inv_rot[2][2] * currentState.zm;
+
+        for (int i = 0; i < 4; i++) {
+            float wx = currentState.feet[i][0];
+            float wy = currentState.feet[i][1];
+            float wz = currentState.feet[i][2];
+
+            float bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0];
+            float by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1];
+            float bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2];
+
+            float mx = mountOffsets[i][0];
+            float my = mountOffsets[i][1];
+            float mz = mountOffsets[i][2];
+
+            float px = bx - mx;
+            float py = by - my;
+            float pz = bz - mz;
+
+            float lx = invMountRot[0][0] * px + invMountRot[0][1] * py + invMountRot[0][2] * pz;
+            float ly = invMountRot[1][0] * px + invMountRot[1][1] * py + invMountRot[1][2] * pz;
+            float lz = invMountRot[2][0] * px + invMountRot[2][1] * py + invMountRot[2][2] * pz;
+
+            float xLocal = (i % 2 == 1) ? -lx : lx;
+            legIK(xLocal, ly, lz, result + i * 3);
+        }
+
+        return ret;
+    }
+
+    inline void euler2R(float roll, float pitch, float yaw, float rot[3][3]) {
+        float cos_roll = std::cos(roll);
+        float sin_roll = std::sin(roll);
+        float cos_pitch = std::cos(pitch);
+        float sin_pitch = std::sin(pitch);
+        float cos_yaw = std::cos(yaw);
+        float sin_yaw = std::sin(yaw);
+
+        rot[0][0] = cos_pitch * cos_yaw;
+        rot[0][1] = -sin_yaw * cos_pitch;
+        rot[0][2] = sin_pitch;
+        rot[1][0] = sin_roll * sin_pitch * cos_yaw + sin_yaw * cos_roll;
+        rot[1][1] = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
+        rot[1][2] = -sin_roll * cos_pitch;
+        rot[2][0] = sin_roll * sin_yaw - sin_pitch * cos_roll * cos_yaw;
+        rot[2][1] = sin_roll * cos_yaw + sin_pitch * sin_yaw * cos_roll;
+        rot[2][2] = cos_roll * cos_pitch;
+    }
+
+    inline void inverse(float rot[3][3], float inv_rot[3][3]) {
+        inv_rot[0][0] = rot[0][0];
+        inv_rot[0][1] = rot[1][0];
+        inv_rot[0][2] = rot[2][0];
+        inv_rot[1][0] = rot[0][1];
+        inv_rot[1][1] = rot[1][1];
+        inv_rot[1][2] = rot[2][1];
+        inv_rot[2][0] = rot[0][2];
+        inv_rot[2][1] = rot[1][2];
+        inv_rot[2][2] = rot[2][2];
+    }
+
+    inline void legIK(float x, float y, float z, float result[3]) {
+        float F = sqrt(max(0.0f, x * x + y * y - l1 * l1));
+        float G = F - l2;
+        float H = sqrt(G * G + z * z);
+
+        float theta1 = -atan2f(y, x) - atan2f(F, -l1);
+        float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4);
+        float theta3 = acosf(max(-1.0f, min(1.0f, D)));
+        float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3));
+        result[0] = RAD_TO_DEG_F(theta1);
+        result[1] = RAD_TO_DEG_F(theta2);
+#if defined(SPOTMICRO_ESP32)
+        result[2] = RAD_TO_DEG_F(theta3);
+#elif defined(SPOTMICRO_YERTLE)
+        result[2] = RAD_TO_DEG_F(theta3 + theta2);
+#endif
+    }
+};
+
+#endif

esp32/include/mdns_service.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <PsychicHttp.h>
+#include <ESPmDNS.h>
+#include <template/stateful_service.h>
+#include <template/stateful_endpoint.h>
+#include <template/stateful_persistence.h>
+#include <settings/mdns_settings.h>
+#include <utils/timing.h>
+
+class MDNSService : public StatefulService<MDNSSettings> {
+  private:
+    FSPersistence<MDNSSettings> _persistence;
+    bool _started;
+
+    void reconfigureMDNS();
+    void startMDNS();
+    void stopMDNS();
+    void addServices();
+
+  public:
+    MDNSService();
+    ~MDNSService();
+
+    void begin();
+
+    esp_err_t getStatus(PsychicRequest *request);
+    void getStatus(JsonObject &root);
+
+    static esp_err_t queryServices(PsychicRequest *request, JsonVariant &json);
+
+    StatefulHttpEndpoint<MDNSSettings> endpoint;
+};

esp32/include/motion.h

@@ -1,7 +1,8 @@
 #ifndef MotionService_h
 #define MotionService_h
 
-#include <event_socket.h>
+#include <event_bus.hpp>
+#include <topic.hpp>
 #include <kinematics.h>
 #include <peripherals/servo_controller.h>
 #include <utils/timing.h>
@@ -21,51 +22,37 @@ enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WA
 
 class MotionService {
   public:
-    MotionService(ServoController *servoController) : _servoController(servoController) {}
+    MotionService(ServoController* servoController) : _servoController(servoController) {}
 
     void begin() {
-        socket.onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
-
-        socket.onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); });
-
-        socket.onEvent(ANGLES_EVENT, [&](JsonObject &root, int originId) { anglesEvent(root, originId); });
-
-        socket.onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); });
-
-        socket.onSubscribe(ANGLES_EVENT,
-                           std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
-
-        body_state.updateFeet(default_feet_positions);
+        setupEventBusSubscriptions();
+        body_state.updateFeet(kinematics.default_feet_positions);
     }
 
-    void anglesEvent(JsonObject &root, int originId) {
-        JsonArray array = root["data"].as<JsonArray>();
+    void anglesEvent(const MotionAnglesMsg& msg) {
         for (int i = 0; i < 12; i++) {
-            angles[i] = array[i];
+            angles[i] = msg.angles[i];
         }
-        syncAngles(String(originId));
+        syncAngles();
     }
 
-    void positionEvent(JsonObject &root, int originId) {
-        JsonArray array = root["data"].as<JsonArray>();
-        body_state.omega = array[0];
-        body_state.phi = array[1];
-        body_state.psi = array[2];
-        body_state.xm = array[3];
-        body_state.ym = array[4];
-        body_state.zm = array[5];
+    void positionEvent(const MotionPositionMsg& msg) {
+        body_state.omega = msg.omega;
+        body_state.phi = msg.phi;
+        body_state.psi = msg.psi;
+        body_state.xm = msg.xm;
+        body_state.ym = msg.ym;
+        body_state.zm = msg.zm;
     }
 
-    void handleInput(JsonObject &root, int originId) {
-        JsonArray array = root["data"].as<JsonArray>();
-        command.lx = array[1];
-        command.lx = array[1];
-        command.ly = array[2];
-        command.rx = array[3];
-        command.ry = array[4];
-        command.h = array[5];
-        command.s = array[6];
-        command.s1 = array[7];
+    void handleInput(const MotionInputMsg& msg) {
+        command.lx = msg.lx;
+        command.ly = msg.ly;
+        command.rx = msg.rx;
+        command.ry = msg.ry;
+        command.h = msg.h;
+        command.s = msg.s;
+        command.s1 = msg.s1;
 
         body_state.ym = (command.h + 127.f) * 0.35f / 100;
 
@@ -75,31 +62,33 @@ class MotionService {
                 body_state.psi = command.ry / 8;
                 body_state.xm = command.ly / 2 / 100;
                 body_state.zm = command.lx / 2 / 100;
-                body_state.updateFeet(default_feet_positions);
+                body_state.updateFeet(kinematics.default_feet_positions);
                 break;
             }
         }
     }
 
-    void handleMode(JsonObject &root, int originId) {
-        motionState = (MOTION_STATE)root["data"].as<int>();
+    void handleMode(const MotionModeMsg& msg) {
+        motionState = (MOTION_STATE)msg.mode;
         ESP_LOGV("MotionService", "Mode %d", motionState);
-        char output[2];
-        itoa((int)motionState, output, 10);
+
         motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
-        socket.emit(MODE_EVENT, output, String(originId).c_str());
+
+        MotionModeMsg response;
+        response.mode = msg.mode;
+        EventBus<MotionModeMsg>::publishAsync(response, _modeHandle);
     }
 
-    void emitAngles(const String &originId = "", bool sync = false) {
-        char output[100];
-        snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
-                 angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
-                 angles[10], angles[11]);
-        socket.emit(ANGLES_EVENT, output, originId.c_str());
+    void emitAngles() {
+        MotionAnglesMsg anglesMsg;
+        for (int i = 0; i < 12; i++) {
+            anglesMsg.angles[i] = angles[i];
+        }
+        EventBus<MotionAnglesMsg>::publishAsync(anglesMsg, _anglesHandle);
     }
 
-    void syncAngles(const String &originId = "", bool sync = false) {
-        emitAngles(originId, sync);
+    void syncAngles() {
+        emitAngles();
         _servoController->setAngles(angles);
     }
 
@@ -134,10 +123,33 @@ class MotionService {
         return updated;
     }
 
-    float *getAngles() { return angles; }
+    float* getAngles() { return angles; }
 
   private:
-    ServoController *_servoController;
+    void setupEventBusSubscriptions() {
+        _inputHandle = EventBus<MotionInputMsg>::subscribe([this](const MotionInputMsg* msg, size_t n) {
+            if (n > 0) handleInput(msg[0]);
+        });
+
+        _modeHandle = EventBus<MotionModeMsg>::subscribe([this](const MotionModeMsg* msg, size_t n) {
+            if (n > 0) handleMode(msg[0]);
+        });
+
+        _anglesHandle = EventBus<MotionAnglesMsg>::subscribe([this](const MotionAnglesMsg* msg, size_t n) {
+            if (n > 0) anglesEvent(msg[0]);
+        });
+
+        _positionHandle = EventBus<MotionPositionMsg>::subscribe([this](const MotionPositionMsg* msg, size_t n) {
+            if (n > 0) positionEvent(msg[0]);
+        });
+    }
+
+    EventBus<MotionInputMsg>::Handle _inputHandle;
+    EventBus<MotionModeMsg>::Handle _modeHandle;
+    EventBus<MotionAnglesMsg>::Handle _anglesHandle;
+    EventBus<MotionPositionMsg>::Handle _positionHandle;
+
+    ServoController* _servoController;
     Kinematics kinematics;
     ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
 
@@ -154,10 +166,13 @@ class MotionService {
     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}};
-
+#if defined(SPOTMICRO_ESP32)
     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};
+#elif defined(SPOTMICRO_YERTLE)
+    float rest_angles[12] = {0, 45, -45, 0, 45, -45, 0, 45, -45, 0, 45, -45};
+    float calibration_angles[12] = {0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0};
+#endif
 };
 
 #endif

esp32/include/msgs/command_msg.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct CommandMsg {
+    float x, y;
+    friend void toJson(JsonVariant v, CommandMsg const &c) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(c.x);
+        arr.add(c.y);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        x = arr[0].as<float>();
+        y = arr[1].as<float>();
+    }
+};

esp32/include/msgs/i2c_scan_msg.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <ArduinoJson.h>
+#include <vector>
+
+struct I2CScanMsg {
+    std::vector<uint8_t> addresses;
+
+    friend void toJson(JsonVariant v, I2CScanMsg const& c) {
+        JsonArray arr = v.to<JsonArray>();
+        for (uint8_t addr : c.addresses) arr.add(addr);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        addresses.clear();
+        for (JsonVariantConst val : o.as<JsonArrayConst>()) addresses.push_back(val.as<uint8_t>());
+    }
+};

esp32/include/msgs/imu_msg.hpp

@@ -0,0 +1,18 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct ImuMsg {
+    float ypr[3];
+    friend void toJson(JsonVariant v, ImuMsg const &a) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(a.ypr[0]);
+        arr.add(a.ypr[1]);
+        arr.add(a.ypr[2]);
+    }
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        ypr[0] = arr[0].as<float>();
+        ypr[1] = arr[1].as<float>();
+        ypr[2] = arr[2].as<float>();
+    }
+};

esp32/include/msgs/mode_msg.hpp

@@ -0,0 +1,10 @@
+#pragma once
+#include <ArduinoJson.h>
+
+enum class MotionState { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK };
+
+struct ModeMsg {
+    MotionState mode;
+    friend void toJson(JsonVariant v, ModeMsg const &m) { v.set(static_cast<int>(m.mode)); }
+    void fromJson(JsonVariantConst o) { mode = (MotionState)o.as<int>(); }
+};

esp32/include/msgs/motion_angles_msg.hpp

@@ -0,0 +1,20 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct MotionAnglesMsg {
+    float angles[12];
+
+    friend void toJson(JsonVariant v, MotionAnglesMsg const &m) {
+        JsonArray arr = v.to<JsonArray>();
+        for (int i = 0; i < 12; i++) {
+            arr.add(m.angles[i]);
+        }
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        for (int i = 0; i < 12 && i < arr.size(); i++) {
+            angles[i] = arr[i].as<float>();
+        }
+    }
+};

esp32/include/msgs/motion_input_msg.hpp

@@ -0,0 +1,28 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct MotionInputMsg {
+    float lx, ly, rx, ry, h, s, s1;
+
+    friend void toJson(JsonVariant v, MotionInputMsg const &m) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(m.lx);
+        arr.add(m.ly);
+        arr.add(m.rx);
+        arr.add(m.ry);
+        arr.add(m.h);
+        arr.add(m.s);
+        arr.add(m.s1);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        lx = arr[0].as<float>();
+        ly = arr[1].as<float>();
+        rx = arr[2].as<float>();
+        ry = arr[3].as<float>();
+        h = arr[4].as<float>();
+        s = arr[5].as<float>();
+        s1 = arr[6].as<float>();
+    }
+};

esp32/include/msgs/motion_mode_msg.hpp

@@ -0,0 +1,10 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct MotionModeMsg {
+    int mode;
+
+    friend void toJson(JsonVariant v, MotionModeMsg const &m) { v.set(m.mode); }
+
+    void fromJson(JsonVariantConst o) { mode = o.as<int>(); }
+};

esp32/include/msgs/motion_position_msg.hpp

@@ -0,0 +1,26 @@
+#pragma once
+#include <ArduinoJson.h>
+
+struct MotionPositionMsg {
+    float omega, phi, psi, xm, ym, zm;
+
+    friend void toJson(JsonVariant v, MotionPositionMsg const &m) {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(m.omega);
+        arr.add(m.phi);
+        arr.add(m.psi);
+        arr.add(m.xm);
+        arr.add(m.ym);
+        arr.add(m.zm);
+    }
+
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        omega = arr[0].as<float>();
+        phi = arr[1].as<float>();
+        psi = arr[2].as<float>();
+        xm = arr[3].as<float>();
+        ym = arr[4].as<float>();
+        zm = arr[5].as<float>();
+    }
+};

esp32/include/msgs/servo_msg.hpp

@@ -0,0 +1,22 @@
+#pragma once
+#include <ArduinoJson.h>
+
+#ifndef NUM_SERVOS
+#define NUM_SERVOS 12
+#endif
+
+struct ServoMsg {
+    float angles[NUM_SERVOS];
+    friend void toJson(JsonVariant v, ServoMsg const &a) {
+        JsonArray arr = v.to<JsonArray>();
+        for (int i = 0; i < NUM_SERVOS; i++) {
+            arr.add(a.angles[i]);
+        }
+    }
+    void fromJson(JsonVariantConst o) {
+        JsonArrayConst arr = o.as<JsonArrayConst>();
+        for (int i = 0; i < NUM_SERVOS; i++) {
+            angles[i] = arr[i].as<float>();
+        }
+    }
+};

esp32/include/msgs/topics.def

@@ -0,0 +1,9 @@
+#define TOPIC_LIST                       \
+    X(Imu, ImuMsg)                       \
+    X(Mode, ModeMsg)                     \
+    X(Command, CommandMsg)               \
+    X(Servo, ServoMsg)                   \
+    X(MotionInput, MotionInputMsg)       \
+    X(MotionAngles, MotionAnglesMsg)     \
+    X(MotionPosition, MotionPositionMsg) \
+    X(MotionMode, MotionModeMsg)

esp32/include/peripherals/imu.h

@@ -6,12 +6,24 @@
 #include <ArduinoJson.h>
 #include <utils/math_utils.h>
 
+#if FT_ENABLED(USE_MPU6050)
 #include <MPU6050_6Axis_MotionApps612.h>
+#endif
+
+#if FT_ENABLED(USE_BNO055)
+#include <Adafruit_BNO055.h>
+#endif
 
 class IMU {
   public:
-    IMU() {}
+    IMU()
+#if FT_ENABLED(USE_BNO055)
+        : _imu(55, 0x29)
+#endif
+    {
+    }
     bool initialize() {
+#if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         imu_success = _imu.testConnection();
         devStatus = _imu.dmpInitialize();
@@ -20,27 +32,46 @@ class IMU {
         _imu.setI2CMasterModeEnabled(false);
         _imu.setI2CBypassEnabled(true);
         _imu.setSleepEnabled(false);
+#endif
+#if FT_ENABLED(USE_BNO055)
+        imu_success = _imu.begin();
+        if (!imu_success) {
+            return false;
+        }
+        _imu.setExtCrystalUse(true);
+#endif
         return true;
     }
 
     bool readIMU() {
         if (!imu_success) return false;
+#if FT_ENABLED(USE_MPU6050)
         bool updated = _imu.dmpGetCurrentFIFOPacket(fifoBuffer);
         _imu.dmpGetQuaternion(&q, fifoBuffer);
         _imu.dmpGetGravity(&gravity, &q);
         _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] *= 180 / M_PI;
+        ypr[1] *= 180 / M_PI;
+        ypr[2] *= 180 / M_PI;
         return updated;
+#endif
+#if FT_ENABLED(USE_BNO055)
+        sensors_event_t event;
+        _imu.getEvent(&event);
+        ypr[0] = (float)event.orientation.x;
+        ypr[1] = (float)event.orientation.y;
+        ypr[2] = (float)event.orientation.z;
+#endif
+        return true;
     }
 
     float getTemperature() { return imu_success ? imu_temperature : -1; }
 
-    float getAngleX() { return imu_success ? ypr[0] * 180 / M_PI : 0; }
+    float getAngleX() { return imu_success ? ypr[0] : 0; }
 
-    float getAngleY() { return imu_success ? ypr[1] * 180 / M_PI : 0; }
+    float getAngleY() { return imu_success ? ypr[1] : 0; }
 
-    float getAngleZ() { return imu_success ? ypr[2] * 180 / M_PI : 0; }
-
-    Quaternion* getQuaternion() { return &q; }
+    float getAngleZ() { return imu_success ? ypr[2] : 0; }
 
     void readIMU(JsonObject& root) {
         if (!imu_success) return;
@@ -52,12 +83,17 @@ class IMU {
     bool active() { return imu_success; }
 
   private:
+#if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
-    bool imu_success {false};
     uint8_t devStatus {false};
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;
+#endif
+#if FT_ENABLED(USE_BNO055)
+    Adafruit_BNO055 _imu;
+#endif
+    bool imu_success {false};
     float ypr[3];
     float imu_temperature {-1};
 };

esp32/include/peripherals/peripherals.h

@@ -55,25 +55,25 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         _eventEndpoint.begin();
         _persistence.readFromFS();
 
-        socket.onEvent(EVENT_I2C_SCAN, [&](JsonObject &root, int originId) {
-            scanI2C();
-            emitI2C();
-        });
+        // socket.onEvent(EVENT_I2C_SCAN, [&](JsonObject &root, int originId) {
+        //     scanI2C();
+        //     emitI2C();
+        // });
 
-        socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
-            scanI2C();
-            emitI2C(originId, sync);
-        });
+        // socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
+        //     scanI2C();
+        //     emitI2C(originId, sync);
+        // });
 
         updatePins();
 
-#if FT_ENABLED(USE_IMU)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
 #endif
 #if FT_ENABLED(USE_USS)
@@ -115,7 +115,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         }
         serializeJson(root, output);
         ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
-        socket.emit(EVENT_I2C_SCAN, output, originId.c_str(), sync);
+        // socket.emit(EVENT_I2C_SCAN, output, originId.c_str(), sync);
     }
 
     void scanI2C(uint8_t lower = 1, uint8_t higher = 127) {
@@ -137,7 +137,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     /* IMU FUNCTIONS */
     bool readIMU() {
         bool updated = false;
-#if FT_ENABLED(USE_IMU)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         beginTransaction();
         updated = _imu.readIMU();
         endTransaction();
@@ -147,7 +147,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     bool readMag() {
         bool updated = false;
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         beginTransaction();
         updated = _mag.readMagnetometer();
         endTransaction();
@@ -157,7 +157,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     bool readBMP() {
         bool updated = false;
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         beginTransaction();
         updated = _bmp.readBarometer();
         endTransaction();
@@ -181,25 +181,24 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     void emitIMU() {
         doc.clear();
         JsonObject root = doc.to<JsonObject>();
-#if FT_ENABLED(USE_IMU)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         _imu.readIMU(root);
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         _mag.readMagnetometer(root);
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         _bmp.readBarometer(root);
 #endif
         serializeJson(doc, message);
-        socket.emit(EVENT_IMU, message);
+        // socket.emit(EVENT_IMU, message);
     }
 
     void emitSonar() {
 #if FT_ENABLED(USE_USS)
-
         char output[16];
         snprintf(output, sizeof(output), "[%.1f,%.1f]", _left_distance, _right_distance);
-        socket.emit("sonar", output);
+        // socket.emit("sonar", output);
 #endif
     }
 
@@ -214,13 +213,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
-#if FT_ENABLED(USE_IMU)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
     Magnetometer _mag;
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
     Barometer _bmp;
 #endif
 #if FT_ENABLED(USE_USS)

esp32/include/peripherals/servo_controller.h

@@ -2,7 +2,7 @@
 #define ServoController_h
 
 #include <Adafruit_PWMServoDriver.h>
-#include <event_socket.h>
+#include <event_bus.hpp>
 #include <template/stateful_persistence.h>
 #include <template/stateful_service.h>
 #include <template/stateful_endpoint.h>
@@ -32,16 +32,16 @@ class ServoController : public StatefulService<ServoSettings> {
           _persistence(ServoSettings::read, ServoSettings::update, this, SERVO_SETTINGS_FILE) {}
 
     void begin() {
-        socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
-                       [&](JsonObject &root, int originId) { servoEvent(root, originId); });
-        socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
+        // socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
+        //                [&](JsonObject &root, int originId) { servoEvent(root, originId); });
+        // socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) { stateUpdate(root, originId); });
         _persistence.readFromFS();
 
         initializePCA();
-        socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
-            is_active = root["active"] | false;
-            is_active ? activate() : deactivate();
-        });
+        // socket.onEvent(EVENT_SERVO_STATE, [&](JsonObject &root, int originId) {
+        //     is_active = root["active"] | false;
+        //     is_active ? activate() : deactivate();
+        // });
     }
 
     void pcaWrite(int index, int value) {
@@ -74,9 +74,15 @@ class ServoController : public StatefulService<ServoSettings> {
 
     void servoEvent(JsonObject &root, int originId) {
         control_state = SERVO_CONTROL_STATE::PWM;
-        uint8_t servo_id = root["servo_id"];
-        int pwm = root["pwm"].as<int>();
-        pcaWrite(servo_id, pwm);
+        int8_t servo_id = root["servo_id"];
+        uint16_t pwm = root["pwm"].as<uint16_t>();
+        if (servo_id < 0) {
+            uint16_t pwms[12];
+            std::fill_n(pwms, 12, pwm);
+            _pca.setMultiplePWM(pwms, 12);
+        } else {
+            _pca.setPWM(servo_id, 0, pwm);
+        }
         ESP_LOGI("SERVO_CONTROLLER", "Setting servo %d to %d", servo_id, pwm);
     }
 
@@ -85,7 +91,7 @@ class ServoController : public StatefulService<ServoSettings> {
         snprintf(output, sizeof(output), "[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", angles[0],
                  angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9],
                  angles[10], angles[11]);
-        socket.emit("angles", output, String(originId).c_str());
+        // socket.emit("angles", output, String(originId).c_str());
     }
 
     void updateActiveState() { is_active ? activate() : deactivate(); }
@@ -98,17 +104,18 @@ class ServoController : public StatefulService<ServoSettings> {
     }
 
     void calculatePWM() {
+        uint16_t pwms[12];
         for (int i = 0; i < 12; i++) {
-            angles[i] = lerp(angles[i], target_angles[i], 0.2);
+            angles[i] = lerp(angles[i], target_angles[i], 0.05);
             auto &servo = state().servos[i];
             float angle = servo.direction * angles[i] + servo.centerAngle;
             uint16_t pwm = angle * servo.conversion + servo.centerPwm;
             if (pwm < 125 || pwm > 600) {
-                ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
                 continue;
             }
-            pcaWrite(i, pwm);
+            pwms[i] = pwm;
         }
+        _pca.setMultiplePWM(pwms, 12);
     }
 
     void updateServoState() {

esp32/include/settings/mdns_settings.h

@@ -0,0 +1,139 @@
+#pragma once
+
+#include <ArduinoJson.h>
+#include <utils/json_utils.h>
+#include <utils/string_utils.h>
+#include <template/state_result.h>
+#include <filesystem.h>
+
+#ifndef FACTORY_MDNS_HOSTNAME
+#define FACTORY_MDNS_HOSTNAME "esp32"
+#endif
+
+#ifndef FACTORY_MDNS_INSTANCE
+#define FACTORY_MDNS_INSTANCE "ESP32 Device"
+#endif
+
+typedef struct {
+    String key;
+    String value;
+
+    void serialize(JsonObject &json) const {
+        json["key"] = key;
+        json["value"] = value;
+    }
+
+    bool deserialize(const JsonObject &json) {
+        key = json["key"].as<String>();
+        value = json["value"].as<String>();
+
+        return key.length() > 0;
+    }
+} mdns_txt_record_t;
+
+typedef struct {
+    String service;
+    String protocol;
+    uint16_t port;
+    std::vector<mdns_txt_record_t> txtRecords;
+
+    void serialize(JsonObject &json) const {
+        json["service"] = service;
+        json["protocol"] = protocol;
+        json["port"] = port;
+
+        if (txtRecords.size() > 0) {
+            JsonArray txtArray = json["txt_records"].to<JsonArray>();
+            for (const auto &txt : txtRecords) {
+                JsonObject txtObj = txtArray.add<JsonObject>();
+                txt.serialize(txtObj);
+            }
+        }
+    }
+
+    bool deserialize(const JsonObject &json) {
+        service = json["service"].as<String>();
+        protocol = json["protocol"].as<String>();
+        port = json["port"] | 0;
+
+        txtRecords.clear();
+        if (json["txt_records"].is<JsonArray>()) {
+            JsonArray txtArray = json["txt_records"];
+            for (JsonObject txtObj : txtArray) {
+                mdns_txt_record_t txt;
+                if (txt.deserialize(txtObj)) {
+                    txtRecords.push_back(txt);
+                }
+            }
+        }
+
+        return service.length() > 0 && protocol.length() > 0 && port > 0;
+    }
+} mdns_service_t;
+
+class MDNSSettings {
+  public:
+    String hostname;
+    String instance;
+    std::vector<mdns_service_t> services;
+    std::vector<mdns_txt_record_t> globalTxtRecords;
+
+    static void read(MDNSSettings &settings, JsonObject &root) {
+        root["hostname"] = settings.hostname;
+        root["instance"] = settings.instance;
+
+        JsonArray servicesArray = root["services"].to<JsonArray>();
+        for (const auto &service : settings.services) {
+            JsonObject serviceObj = servicesArray.add<JsonObject>();
+            service.serialize(serviceObj);
+        }
+
+        JsonArray txtArray = root["global_txt_records"].to<JsonArray>();
+        for (const auto &txt : settings.globalTxtRecords) {
+            JsonObject txtObj = txtArray.add<JsonObject>();
+            txt.serialize(txtObj);
+        }
+    }
+
+    static StateUpdateResult update(JsonObject &root, MDNSSettings &settings) {
+        settings.hostname = root["hostname"] | FACTORY_MDNS_HOSTNAME;
+        settings.instance = root["instance"] | FACTORY_MDNS_INSTANCE;
+
+        settings.services.clear();
+        if (root["services"].is<JsonArray>()) {
+            JsonArray servicesArray = root["services"];
+            for (JsonObject serviceObj : servicesArray) {
+                mdns_service_t service;
+                if (service.deserialize(serviceObj)) {
+                    settings.services.push_back(service);
+                }
+            }
+        }
+
+        if (settings.services.empty()) {
+            mdns_service_t httpService = {.service = "http", .protocol = "tcp", .port = 80};
+            settings.services.push_back(httpService);
+
+            mdns_service_t wsService = {.service = "ws", .protocol = "tcp", .port = 80};
+            settings.services.push_back(wsService);
+        }
+
+        settings.globalTxtRecords.clear();
+        if (root["global_txt_records"].is<JsonArray>()) {
+            JsonArray txtArray = root["global_txt_records"];
+            for (JsonObject txtObj : txtArray) {
+                mdns_txt_record_t txt;
+                if (txt.deserialize(txtObj)) {
+                    settings.globalTxtRecords.push_back(txt);
+                }
+            }
+        }
+
+        if (settings.globalTxtRecords.empty()) {
+            mdns_txt_record_t firmwareVersion = {.key = "Firmware Version", .value = APP_VERSION};
+            settings.globalTxtRecords.push_back(firmwareVersion);
+        }
+
+        return StateUpdateResult::CHANGED;
+    }
+};

esp32/include/settings/peripherals_settings.h

@@ -14,7 +14,7 @@
 #define SCL_PIN SCL
 #endif
 #ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 100000UL
+#define I2C_FREQUENCY 1000000UL
 #endif
 
 class PinConfig {