From e0fa434aeb7e9dd60d63b534e344175ef5d50fba Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 30 Mar 2024 16:50:39 +0100 Subject: [PATCH] Allow both way sync of angles --- app2/src/lib/stores/model-store.ts | 9 +++- app2/src/routes/+layout.svelte | 11 +++- app2/src/routes/controller/Controls.svelte | 8 +-- esp32/src/ActuatorStateService.cpp | 13 ++++- esp32/src/ActuatorStateService.h | 58 +++++++++++++++++++--- esp32/src/main.cpp | 5 +- 6 files changed, 87 insertions(+), 17 deletions(-) diff --git a/app2/src/lib/stores/model-store.ts b/app2/src/lib/stores/model-store.ts index 3129e36..5474989 100644 --- a/app2/src/lib/stores/model-store.ts +++ b/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 = writable('idle'); +export enum ModesEnum { + Idle, + Rest, + Stand, + Walk +} + +export const mode: Writable = writable(ModesEnum.Idle); export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0])); diff --git a/app2/src/routes/+layout.svelte b/app2/src/routes/+layout.svelte index 733b302..5a4697c 100644 --- a/app2/src/routes/+layout.svelte +++ b/app2/src/routes/+layout.svelte @@ -29,7 +29,7 @@ connectToEventSource(); connectToSocket() addPublisher(outControllerData, "controller") - addPublisher(mode, "mode") + addPublisher(mode as unknown as Writable, "mode") addPublisher(servoAngles as unknown as Writable, "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) => { diff --git a/app2/src/routes/controller/Controls.svelte b/app2/src/routes/controller/Controls.svelte index 8d9356c..65137a9 100644 --- a/app2/src/routes/controller/Controls.svelte +++ b/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)); }; @@ -89,12 +89,12 @@
{#each modes as modeValue} - {/each}
- {#if $mode === 'walk'} + {#if $mode === ModesEnum.Walk} handleRange(e, 'speed')} class="range range-sm" /> {/if} diff --git a/esp32/src/ActuatorStateService.cpp b/esp32/src/ActuatorStateService.cpp index ceb5671..35f46e6 100644 --- a/esp32/src/ActuatorStateService.cpp +++ b/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() diff --git a/esp32/src/ActuatorStateService.h b/esp32/src/ActuatorStateService.h index 8f0aecb..48bf84e 100644 --- a/esp32/src/ActuatorStateService.h +++ b/esp32/src/ActuatorStateService.h @@ -17,14 +17,29 @@ #include #include +#include #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()) return StateUpdateResult::UNCHANGED; + actuatorState.motionState = (MOTION_STATE)root["data"].as(); + 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()) + if (actuatorState.angles[i] != array[i].as()) { - 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()) + if (actuatorState.angles[i] != array[i].as()) { - actuatorState.state[i] = array[i]; + actuatorState.angles[i] = array[i]; changed = true; } } @@ -81,9 +101,31 @@ public: class ActuatorStateService : public StatefulService { 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(_this)->_loop(); } + void _loop() + { + TickType_t xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + while (1) + { + _state.angles[1] = (_state.angles[1] + 5) % 90; + StaticJsonDocument 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 _httpEndpoint; WebSocketServer _webSocketServer; diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index dd0223e..8d48bac 100644 --- a/esp32/src/main.cpp +++ b/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()