🏍️ Adds motionservice with data sync

app/src/lib/components/Visualization.svelte

@@ -3,7 +3,7 @@
 	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
 	import uzip from 'uzip';
 	import { model, servoAnglesOut } from '$lib/stores';
-	import { footColor, isEmbeddedApp, toeWorldPositions } from '$lib/utilities';
+	import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
 	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
@@ -22,6 +22,7 @@
 	let currentModelAngles: number[] = new Array(12).fill(0);
 	let modelTargetAngles: number[] = new Array(12).fill(0)
     let gui_panel: GUI
+    let Throttler = new throttler()
 
     let feet_trace = new Array(4).fill([]);
     let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
@@ -72,7 +73,7 @@
 
 	const updateAngles = (name: string, angle: number) => {
 		modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
-        servoAnglesOut.set(modelTargetAngles)
+        Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles), 100)
 	};
 
 	const createScene = async () => {

app/src/lib/stores/model-store.ts

@@ -21,7 +21,7 @@ export enum ModesEnum {
 
 export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
 
-export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
+export const outControllerData = writable(new Array([0, 0, 0, 0, 0, 70, 0]));
 
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },

app/src/routes/+layout.svelte

@@ -13,7 +13,7 @@
 	import Menu from './menu.svelte';
 	import Statusbar from './statusbar.svelte';
 	import Login from './login.svelte';
-	import { mode, outControllerData, servoAnglesOut, socket } from '$lib/stores';
+	import { ModesEnum, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores';
 	import type { Analytics, Battery, DownloadOTA } from '$lib/types/models';
 
 	onMount(async () => {
@@ -25,9 +25,9 @@
 
 		addEventListeners();
 
-        outControllerData.subscribe((data) => socket.sendEvent("input", data));
+        outControllerData.subscribe((data) => socket.sendEvent("input", {data}));
-        mode.subscribe((data) => socket.sendEvent("mode", data));
+        mode.subscribe((data) => socket.sendEvent("mode", {data}));
-        servoAnglesOut.subscribe((data) => socket.sendEvent("angles", data));
+        servoAnglesOut.subscribe((data) => socket.sendEvent("angles", {data}));
 	});
 
     onDestroy(() => {
@@ -43,6 +43,8 @@
 		socket.on('successToast', handleSuccessToast);
 		socket.on('warningToast', handleWarningToast);
 		socket.on('errorToast', handleErrorToast);
+		socket.on('mode', (data:ModesEnum) => mode.set(data));
+		socket.on('angles', (angles:number[]) => { if (angles.length) servoAngles.set(angles)});
 		if ($page.data.features.analytics) socket.on('analytics', handleAnalytics);
 		if ($page.data.features.battery) socket.on('battery', handleBattery);
 		if ($page.data.features.download_firmware) socket.on('otastatus', handleOAT);

app/src/routes/controller/+layout.svelte

@@ -1,14 +1,16 @@
 <script lang="ts">
 	import Controls from './Controls.svelte';
 	import { socket } from '$lib/stores';
+	import Spinner from '$lib/components/Spinner.svelte';
 </script>
 <div>
-    {#if $socket}
+    {#if !$socket}
-        <Controls />
+    <div class="flex flex-col h-full justify-center items-center">
-        <slot/>
+        <Spinner/>
-    {:else}
-        <div class="flex justify-center items-center">
             <h2>Waiting for connection</h2>
         </div>
+    {:else}
+        <Controls />
+        <slot/>
     {/if}
 </div>

app/src/routes/controller/Controls.svelte

@@ -10,7 +10,7 @@
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-	let data = new Int8Array(7);
+	let data = new Array(7);
 
 	onMount(() => {
 		left = nipplejs.create({

esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp

@@ -53,7 +53,8 @@ ESP32SvelteKit::ESP32SvelteKit(PsychicHttpServer *server, unsigned int numberEnd
       _restartService(server, &_securitySettingsService),
       _factoryResetService(server, &ESPFS, &_securitySettingsService),
       _systemStatus(server, &_securitySettingsService),
-      _fileExplorer(server, &_securitySettingsService){
+      _fileExplorer(server, &_securitySettingsService),
+      _motionService(_server, &_socket, &_securitySettingsService, &_taskManager) {
 }
 
 void ESP32SvelteKit::begin() {
@@ -181,6 +182,7 @@ void ESP32SvelteKit::startServices() {
 #endif
   _taskManager.begin();
   _fileExplorer.begin();
+  _motionService.begin();
 }
 
 void IRAM_ATTR ESP32SvelteKit::_loop() {

esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h

@@ -32,6 +32,7 @@
 #include <FeaturesService.h>
 #include <MqttSettingsService.h>
 #include <MqttStatus.h>
+#include <MotionService.h>
 #include <NTPSettingsService.h>
 #include <NTPStatus.h>
 #include <PsychicHttp.h>
@@ -159,6 +160,11 @@ public:
         return &_fileExplorer;
     }
 
+    MotionService *getMotionService()
+    {
+        return &_motionService;
+    }
+
     void factoryReset()
     {
         _factoryResetService.factoryReset();
@@ -216,6 +222,7 @@ private:
     SystemStatus _systemStatus;
     TaskManager _taskManager;
     FileExplorer _fileExplorer;
+    MotionService _motionService;
 
     String _appName = APP_NAME;
 

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -0,0 +1,103 @@
+#ifndef MotionService_h
+#define MotionService_h
+
+#include <EventSocket.h>
+#include <TaskManager.h>
+
+#define DEFAULT_STATE false
+#define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
+#define ANGLES_EVENT "angles"
+#define INPUT_EVENT "input"
+#define MODE_EVENT "mode"
+
+#define MOTION_INTERVAL 100
+
+enum class MOTION_STATE
+{
+    IDLE,
+    REST,
+    STAND,
+    WALK
+};
+
+class MotionService
+{
+  public:
+    MotionService(PsychicHttpServer *server, EventSocket *socket, SecurityManager *securityManager, TaskManager *taskManager)
+        : _server(server), _socket(socket), _securityManager(securityManager), _taskManager(taskManager)
+    {
+    }
+
+    void begin()
+    {
+        _socket->registerEvent(INPUT_EVENT);
+        _socket->registerEvent(ANGLES_EVENT);
+        _socket->registerEvent(MODE_EVENT);
+
+        _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->onSubscribe(ANGLES_EVENT,
+                             std::bind(&MotionService::syncState, this, std::placeholders::_1, std::placeholders::_2));
+    }
+
+    void syncState(const String &originId, bool sync = false)
+    {
+        DynamicJsonDocument jsonDocument{200};
+        char output[200];
+        JsonObject root = jsonDocument.to<JsonObject>();
+        root["angles"] = angles;
+        serializeJson(root, output);
+        ESP_LOGV("MotionState", "Syncing state: %s", output);
+        _socket->emit(ANGLES_EVENT, output, originId.c_str());
+    }
+
+    void anglesEvent(JsonObject &root, int originId)
+    {
+        JsonArray array = root["data"].as<JsonArray>();
+        for (int i = 0; i < 12; i++)
+        {
+            angles[i] = array[i];
+        }
+        char output[100];
+        serializeJson(array, output);
+        sprintf(output, "[%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d]", 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, String(originId).c_str());
+    }
+
+    void handleInput(JsonObject &root, int originId)
+    {
+        String jsonString;
+        JsonArray array = root["data"].as<JsonArray>();
+        serializeJson(array, jsonString);
+        ESP_LOGI("MotionService", "%s", jsonString.c_str());
+        for (int i = 0; i < 7; i++)
+        {
+            input[i] = array[i];
+        }
+        ESP_LOGI("MotionService", "Input: %d %d %d %d %d %d %d", input[0], input[1], input[2], input[3], input[4], input[5], input[6]);
+    }
+
+    void handleMode(JsonObject &root, int originId)
+    {
+        ESP_LOGV("MotionService", "Mode %d", root["data"].as<int>());
+        motionState = (MOTION_STATE)root["data"].as<int>();
+        char output[2];
+        sprintf(output, "%d", motionState);
+        _socket->emit(MODE_EVENT, output, String(originId).c_str());
+    }
+
+  private:
+    PsychicHttpServer *_server;
+    EventSocket *_socket;
+    SecurityManager *_securityManager;
+    TaskManager *_taskManager;
+    int8_t input[7] = {0, 0, 0, 0, 0, 0, 0};
+    int16_t angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    MOTION_STATE motionState = MOTION_STATE::IDLE;
+};
+
+#endif