From 9dee0e1bb19146fdec8503c0518859b2bff51d5b Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 2 May 2024 22:28:47 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8F=8D=EF=B8=8F=20Adds=20motionservice=20?= =?UTF-8?q?with=20data=20sync?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/components/Visualization.svelte | 5 +- app/src/lib/stores/model-store.ts | 2 +- app/src/routes/+layout.svelte | 10 +- app/src/routes/controller/+layout.svelte | 12 ++- app/src/routes/controller/Controls.svelte | 2 +- esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp | 4 +- esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h | 7 ++ esp32/lib/ESP32-sveltekit/MotionService.h | 103 +++++++++++++++++++ 8 files changed, 131 insertions(+), 14 deletions(-) create mode 100644 esp32/lib/ESP32-sveltekit/MotionService.h diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 4a2edf9..b1c6500 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/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[] = [] @@ -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 () => { diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index 5474989..7b41f9f 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -21,7 +21,7 @@ export enum ModesEnum { export const mode: Writable = 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 = writable({ left: { x: 0, y: 0 }, diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte index 73bebab..2f823c1 100644 --- a/app/src/routes/+layout.svelte +++ b/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)); - mode.subscribe((data) => socket.sendEvent("mode", data)); - servoAnglesOut.subscribe((data) => socket.sendEvent("angles", data)); + outControllerData.subscribe((data) => socket.sendEvent("input", {data})); + mode.subscribe((data) => socket.sendEvent("mode", {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); diff --git a/app/src/routes/controller/+layout.svelte b/app/src/routes/controller/+layout.svelte index 3a8517f..6e10505 100644 --- a/app/src/routes/controller/+layout.svelte +++ b/app/src/routes/controller/+layout.svelte @@ -1,14 +1,16 @@
- {#if $socket} - - - {:else} -
+ {#if !$socket} +
+

Waiting for connection

+ {:else} + + {/if}
diff --git a/app/src/routes/controller/Controls.svelte b/app/src/routes/controller/Controls.svelte index 6f26b1c..d4ae87e 100644 --- a/app/src/routes/controller/Controls.svelte +++ b/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({ diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp index 6e3fd36..49c9e96 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.cpp +++ b/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() { diff --git a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h index e65fd09..795fdce 100644 --- a/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h +++ b/esp32/lib/ESP32-sveltekit/ESP32SvelteKit.h @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h new file mode 100644 index 0000000..4a17ca4 --- /dev/null +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -0,0 +1,103 @@ +#ifndef MotionService_h +#define MotionService_h + +#include +#include + +#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(); + 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(); + 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(); + 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()); + motionState = (MOTION_STATE)root["data"].as(); + 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