Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🏍️ Adds motionservice with data sync

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-02 22:28:47 +02:00
committed by Rune Harlyk
parent ae1cb70710
commit 9dee0e1bb1
8 changed files with 131 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+3 -2
View File
@@ -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
+1 -1
View File
@@ -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
+6 -4
View File
@@ -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);
app/src/routes/controller/+layout.svelte
+7 -5
View File
@@ -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}
<Controls />
<slot/>
{:else}
<div class="flex justify-center items-center">
{#if !$socket}
<div class="flex flex-col h-full justify-center items-center">
<Spinner/>
<h2>Waiting for connection</h2>
</div>
{:else}
<Controls />
<slot/>
{/if}
</div>
app/src/routes/controller/Controls.svelte
+1 -1
View File
@@ -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
+3 -1
View File
@@ -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
+7
View File
@@ -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
+103
View File
@@ -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