Allow both way sync of angles

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

@@ -12,7 +12,14 @@ export const modes = ['idle', 'rest', 'stand', 'walk'] as const;
 
 export type Modes = (typeof modes)[number];
 
-export const mode: Writable<Modes> = writable('idle');
+export enum ModesEnum {
+	Idle,
+	Rest,
+	Stand,
+	Walk
+}
+
+export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
 
 export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
 

app2/src/routes/+layout.svelte

@@ -29,7 +29,7 @@
 		connectToEventSource();
         connectToSocket()
         addPublisher(outControllerData, "controller")
-        addPublisher(mode, "mode")
+        addPublisher(mode as unknown as Writable<WebsocketOutData>, "mode")
         addPublisher(servoAngles as unknown as Writable<WebsocketOutData>, "angles")
 	});
 
@@ -127,6 +127,15 @@
 			false
 		);
 
+        NotificationSource.addEventListener(
+			'motion',
+			(event) => {
+                const data = JSON.parse(event.data);
+                servoAngles.set(data.angles);
+			},
+			false
+		);
+
 		NotificationSource.addEventListener(
 			'error',
 			(event) => {

app2/src/routes/controller/Controls.svelte

@@ -2,7 +2,7 @@
 	import nipplejs from 'nipplejs';
 	import { onMount } from 'svelte';
 	import { capitalize, throttler, toInt8 } from '$lib/utilities';
-	import { input, outControllerData, mode, modes, type Modes } from '$lib/stores';
+	import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores';
 	import type { vector } from '$lib/models';
 
 	let throttle = new throttler();
@@ -77,7 +77,7 @@
     }
 
 	const changeMode = (modeValue: Modes) => {
-		mode.set(modeValue);
+		mode.set(modes.indexOf(modeValue));
 	};
 </script>
 
@@ -89,12 +89,12 @@
 	</div>
 	<div class="absolute bottom-0 z-10 p-4 gap-4 flex items-end">
 		{#each modes as modeValue}
-            <button class="btn btn-outline" class:btn-active={$mode === modeValue} on:click={() => changeMode(modeValue)}>
+            <button class="btn btn-outline" class:btn-active={$mode === modes.indexOf(modeValue)} on:click={() => changeMode(modeValue)}>
                 {capitalize(modeValue)}
             </button>
 		{/each}
         <div>
-            {#if $mode === 'walk'}
+            {#if $mode === ModesEnum.Walk}
                 <label for="speed">Speed</label>
                 <input type="range" name="speed" min="0" max="100" on:input={(e) => handleRange(e, 'speed')} class="range range-sm" />
             {/if}

esp32/src/ActuatorStateService.cpp

@@ -16,6 +16,7 @@
 
 ActuatorStateService::ActuatorStateService(
     PsychicHttpServer *server,
+    NotificationEvents *notificationEvents,
     SecurityManager *securityManager
     ) : _httpEndpoint(
             ActuatorState::read,
@@ -32,7 +33,8 @@ ActuatorStateService::ActuatorStateService(
         server,
         ACTUATOR_SETTINGS_SOCKET_PATH,
         securityManager,
-        AuthenticationPredicates::IS_AUTHENTICATED)
+        AuthenticationPredicates::IS_AUTHENTICATED),
+     _notificationEvents(notificationEvents)
 {
     // Setup actuator hardware
 
@@ -44,6 +46,15 @@ void ActuatorStateService::begin()
     _httpEndpoint.begin();
     _webSocketServer.begin();
     onConfigUpdated();
+    xTaskCreatePinnedToCore(
+        this->_loopImpl,            // Function that should be called
+        "Motion Service",           // Name of the task (for debugging)
+        4096,                       // Stack size (bytes)
+        this,                       // Pass reference to this class instance
+        (tskIDLE_PRIORITY + 1),     // task priority
+        NULL,                       // Task handle
+        ESP32SVELTEKIT_RUNNING_CORE // Pin to application core
+    );
 }
 
 void ActuatorStateService::onConfigUpdated()

esp32/src/ActuatorStateService.h

@@ -17,14 +17,29 @@
 
 #include <HttpEndpoint.h>
 #include <WebSocketServer.h>
+#include <NotificationEvents.h>
 
 #define ACTUATOR_SETTINGS_ENDPOINT_PATH "/rest/actuators"
 #define ACTUATOR_SETTINGS_SOCKET_PATH "/ws"
 
+#define MOTION_INTERVAL 100
+
+#define MAX_ESP_ANGLE_SIZE 256
+
+enum class MOTION_STATE
+{
+    IDLE,
+    REST,
+    STAND,
+    WALK
+};
+
 class ActuatorState
 {
 public:
-    int16_t state[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
+    int16_t angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
+    int8_t controller[7] = {0, 0, 0, 0, 0, 0 ,0};
+    MOTION_STATE motionState = MOTION_STATE::IDLE;
 
     static void read(ActuatorState &settings, JsonObject &root)
     {
@@ -32,20 +47,25 @@ public:
         JsonArray array = root.createNestedArray("data");
         for(int i = 0; i < 12; i++)
         {
-            array.add(settings.state[i]);
+            array.add(settings.angles[i]);
         }
     }
 
     static StateUpdateResult update(JsonObject &root, ActuatorState &actuatorState)
     {
+        if (root["type"] == "mode") {
+            if (actuatorState.motionState == (MOTION_STATE)root["data"].as<int>()) return StateUpdateResult::UNCHANGED;
+            actuatorState.motionState = (MOTION_STATE)root["data"].as<int>();
+            return StateUpdateResult::UNCHANGED;
+        }
         if (root["type"] != "angles") return StateUpdateResult::UNCHANGED;
         JsonArray array = root["data"];
         bool changed = false;
         for(int i = 0; i < 12; i++)
         {
-            if (actuatorState.state[i] != array[i].as<int16_t>())
+            if (actuatorState.angles[i] != array[i].as<int16_t>())
             {
-                actuatorState.state[i] = array[i];
+                actuatorState.angles[i] = array[i];
                 //changed = true;
             }
         }
@@ -57,7 +77,7 @@ public:
         JsonArray array = root.createNestedArray("angles");
         for(int i = 0; i < 12; i++)
         {
-            array.add(settings.state[i]);
+            array.add(settings.angles[i]);
         }
     }
 
@@ -69,9 +89,9 @@ public:
         bool changed = false;
         for(int i = 0; i < 12; i++)
         {
-            if (actuatorState.state[i] != array[i].as<int16_t>())
+            if (actuatorState.angles[i] != array[i].as<int16_t>())
             {
-                actuatorState.state[i] = array[i];
+                actuatorState.angles[i] = array[i];
                 changed = true;
             }
         }
@@ -81,9 +101,31 @@ public:
 
 class ActuatorStateService : public StatefulService<ActuatorState> {
     public:
-        ActuatorStateService(PsychicHttpServer *server, SecurityManager *securityManager);
+        ActuatorStateService(PsychicHttpServer *server, NotificationEvents *notificationEvents, SecurityManager *securityManager);
         void begin();
+    protected:
+        NotificationEvents *_notificationEvents;
+        static void _loopImpl(void *_this) { static_cast<ActuatorStateService *>(_this)->_loop(); }
+        void _loop()
+        {
+            TickType_t xLastWakeTime;
+            xLastWakeTime = xTaskGetTickCount();
+            while (1)
+            {
+                _state.angles[1] = (_state.angles[1] + 5) % 90;
+                StaticJsonDocument<MAX_ESP_ANGLE_SIZE> doc;
+                String message;
+                JsonArray array = doc.createNestedArray("angles");
+                for(int16_t num : _state.angles) {
+                    array.add(num);
+                }
+                doc["mode"] = (int)_state.motionState;
 
+                serializeJson(doc, message);
+                _notificationEvents->send(message, "motion", millis());
+                vTaskDelayUntil(&xLastWakeTime, MOTION_INTERVAL / portTICK_PERIOD_MS);
+            }
+        };
     private:
         HttpEndpoint<ActuatorState> _httpEndpoint;
         WebSocketServer<ActuatorState> _webSocketServer;

esp32/src/main.cpp

@@ -6,9 +6,10 @@
 
 DRAM_ATTR PsychicHttpServer server;
 
-DRAM_ATTR ESP32SvelteKit esp32sveltekit(&server, 120);
+DRAM_ATTR ESP32SvelteKit esp32sveltekit(&server, 125);
+
+ActuatorStateService actuatorStateService = ActuatorStateService(&server, esp32sveltekit.getNotificationEvents(), esp32sveltekit.getSecurityManager());
 
-ActuatorStateService actuatorStateService = ActuatorStateService(&server, esp32sveltekit.getSecurityManager());
 
 
 void setup()