diff --git a/app/src/App.svelte b/app/src/App.svelte index 47456fc..06e56f8 100644 --- a/app/src/App.svelte +++ b/app/src/App.svelte @@ -6,7 +6,7 @@ import Controller from './routes/Controller.svelte'; import { fileService } from '$lib/services'; import Settings from './routes/Settings.svelte'; - import { jointNames, model } from '$lib/store'; + import { jointNames, model, outControllerData } from '$lib/store'; import { loadModelAsync } from '$lib/utilities'; import { socketLocation } from '$lib/utilities'; import type { Result } from '$lib/utilities/result'; @@ -14,6 +14,7 @@ export let url = window.location.pathname; onMount(async () => { socketService.connect(socketLocation); + socketService.addPublisher(outControllerData); registerFetchIntercept(); const modelRes = await loadModelAsync('/spot_micro.urdf.xacro'); diff --git a/app/src/components/Controls.svelte b/app/src/components/Controls.svelte index 52ce9f5..557bdf8 100644 --- a/app/src/components/Controls.svelte +++ b/app/src/components/Controls.svelte @@ -1,7 +1,7 @@ diff --git a/app/src/components/Views/Model.svelte b/app/src/components/Views/Model.svelte index 31294d9..b39ae7e 100644 --- a/app/src/components/Views/Model.svelte +++ b/app/src/components/Views/Model.svelte @@ -3,13 +3,13 @@ import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial } from 'three'; import socketService from '$lib/services/socket-service'; import uzip from 'uzip'; - import { model, outControllerData } from '$lib/store'; + import { model } from '$lib/store'; import { ForwardKinematics } from '$lib/kinematic'; import { location } from '$lib/utilities'; import { fileService } from '$lib/services'; import { servoAngles, mpu } from '$lib/stores'; import SceneBuilder from '$lib/sceneBuilder'; - import { lerp } from 'three/src/math/MathUtils'; + import { lerp, degToRad } from 'three/src/math/MathUtils'; let sceneManager: SceneBuilder; let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement; @@ -47,21 +47,9 @@ psi: number; } - const degToRad = (val: number) => val * (Math.PI / 180); - onMount(async () => { await cacheModelFiles(); await createScene(); - - outControllerData.subscribe((data) => { - socketService.send( - JSON.stringify({ - type: 'kinematic/bodystate', - angles: [0, (data[1] - 128) / 3, (data[2] - 128) / 4], - position: [(data[4] - 128) / 2, data[5], (data[3] - 128) / 2] - }) - ); - }); }); onDestroy(() => { diff --git a/app/src/lib/services/socket-service.ts b/app/src/lib/services/socket-service.ts index b0f2539..03a08a7 100644 --- a/app/src/lib/services/socket-service.ts +++ b/app/src/lib/services/socket-service.ts @@ -2,6 +2,7 @@ import { isConnected, socketData } from '$lib/stores'; import { Result, Ok } from '$lib/utilities'; import { resultService } from '$lib/services'; import { type WebSocketJsonMsg } from '$lib/models'; +import type { Writable } from 'svelte/store'; type WebsocketOutData = string | ArrayBufferLike | Blob | ArrayBufferView; @@ -37,6 +38,10 @@ class SocketService { return Result.err('The connection is not open'); } + public addPublisher(store: Writable, type?: string) { + store.subscribe((data) => this.send(type ? JSON.stringify({ type, data }) : data)); + } + private handleConnected(): void { isConnected.set(true); } diff --git a/app/src/lib/store.ts b/app/src/lib/store.ts index fececc1..8d4bdb1 100644 --- a/app/src/lib/store.ts +++ b/app/src/lib/store.ts @@ -10,7 +10,7 @@ export const input = writable({ speed: 0 }); -export const outControllerData = writable(new Uint8Array([0, 128, 128, 128, 128, 70, 0])); +export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0])); export const jointNames = persistentStore('joint_names', []); diff --git a/app/src/lib/utilities/math-utilities.ts b/app/src/lib/utilities/math-utilities.ts index 50404aa..e4e8996 100644 --- a/app/src/lib/utilities/math-utilities.ts +++ b/app/src/lib/utilities/math-utilities.ts @@ -3,3 +3,9 @@ export const toUint8 = (number: number, min: number, max: number) => { let scaled = ((number - min) / (max - min)) * 255; return Math.round(scaled) & 0xff; }; + +export const toInt8 = (number: number, min: number, max: number) => { + number = Math.max(min, Math.min(max, number)); + let scaled = ((number - min) / (max - min)) * 255 - 128; + return Math.max(-128, Math.min(127, Math.round(scaled))) | 0; +}; \ No newline at end of file diff --git a/app/test/specs/number-convert.spec.ts b/app/test/specs/number-convert.spec.ts new file mode 100644 index 0000000..6499494 --- /dev/null +++ b/app/test/specs/number-convert.spec.ts @@ -0,0 +1,46 @@ +import { describe, it, expect } from 'vitest'; +import { toUint8, toInt8 } from '../../src/lib/utilities'; + +describe('toUint8', () => { + it('min interval value should get 0', () => { + expect(toUint8(-1, -1, 1)).toBe(0); + }); + it('middle interval value should get 128', () => { + expect(toUint8(0, -1, 1)).toBe(128); + }); + + it('max interval value should get 255', () => { + expect(toUint8(1, -1, 1)).toBe(255); + }); + + it('min value should be clamped', () => { + expect(toUint8(-2, -1, 1)).toBe(0); + }); + + it('max value should be clamped', () => { + expect(toUint8(2, -1, 1)).toBe(255); + }); +}); + +describe('toInt8', () => { + it('min interval value should get -128', () => { + expect(toInt8(-1, -1, 1)).toBe(-128); + }); + it('middle interval value should get 0', () => { + expect(toInt8(0, -1, 1)).toBe(0); + }); + + it('max interval value should get 127', () => { + expect(toInt8(1, -1, 1)).toBe(127); + }); + + it('min value should be clamped', () => { + expect(toInt8(-2, -1, 1)).toBe(-128); + }); + + it('max value should be clamped', () => { + expect(toInt8(2, -1, 1)).toBe(127); + }); +}); + + diff --git a/app/test/specs/toUint8.spec.ts b/app/test/specs/toUint8.spec.ts deleted file mode 100644 index a4f70f1..0000000 --- a/app/test/specs/toUint8.spec.ts +++ /dev/null @@ -1,24 +0,0 @@ -import { describe, it, expect } from 'vitest'; -import { toUint8 } from '../../src/lib/utilities'; - -describe('toUint8', () => { - it('min interval value should get 0', () => { - expect(toUint8(-1, -1, 1)).toBe(0); - }); - - it('middle interval value should get 128', () => { - expect(toUint8(0, -1, 1)).toBe(128); - }); - - it('max interval value should get 255', () => { - expect(toUint8(1, -1, 1)).toBe(255); - }); - - it('min value should be clamped', () => { - expect(toUint8(-2, -1, 1)).toBe(0); - }); - - it('max value should be clamped', () => { - expect(toUint8(2, -1, 1)).toBe(255); - }); -}); diff --git a/mock/server.js b/mock/server.js index deeb2ae..6ccf222 100644 --- a/mock/server.js +++ b/mock/server.js @@ -89,6 +89,7 @@ const model = { rssi: 100, }, running: true, + mode: "stand", }; const settings = { @@ -197,99 +198,170 @@ const updateAngles = (angles) => { return model.servos.angles; }; +const bufferToController = (buffer) => { + return { + stop: buffer[0], + lx: buffer[1], + ly: buffer[2], + rx: buffer[3], + ry: buffer[4], + h: buffer[5], + s: buffer[6], + }; +}; + +const unpackMessageBuffer = (data) => { + return { + angles: [0, data.rx / 4, data.ry / 4], + position: [data.ly / 2, 70, data.lx / 2], + }; +}; + +const updateStanding = (ws, controller) => { + if (!ws.clientState.model.running) return; + const data = unpackMessageBuffer(controller); + ws.send( + JSON.stringify({ + type: "angles", + data: updateBodyState(ws.clientState.model, data.angles, data.position), + }) + ); +}; + +const handelController = (ws, buffer) => { + const controllerData = bufferToController(new Int8Array(buffer)); + if (controllerData.stop) { + ws.clientState.model.running = false; + ws.clientState.logs.push("[2024-02-05 19:10:00] [Warning] STOPPING SERVOS"); + ws.send(JSON.stringify({ type: "log", data: ws.clientState.logs.last() })); + return; + } + if (ws.clientState.model.mode === "stand") { + updateStanding(ws, controllerData); + } +}; + +const handleBufferMessage = (ws, buffer) => { + if (buffer.length === 6) { + handelController(ws, buffer); + } +}; + +const handleJsonMessage = (ws, message) => { + let data = message; + try { + data = JSON.parse(message); + } catch (error) { + return; + } + switch (data.type) { + case "subscribe": + subscribeClientToCategory(ws, data.category); + break; + case "unsubscribe": + unsubscribeClientFromCategory(ws, data.category); + break; + case "sensor/battery": + ws.send({ type: "battery", data: JSON.stringify(updateBattery()) }); + break; + case "sensor/mpu": + ws.send({ type: "battery", data: JSON.stringify(updateMpu()) }); + break; + case "sensor/distances": + ws.send(JSON.stringify(updateDistances())); + break; + case "sensor/distance": + ws.send(JSON.stringify({ distance: updateDistance(data.position) })); + break; + case "kinematic/angle": + if (data.angle && data.id) { + ws.clientState.model.servos.angles[data.id] = data.angle; + ws.send( + JSON.stringify({ + type: "angles", + data: ws.clientState.model.servos.angles, + }) + ); + } else { + ws.send(JSON.stringify(updateAngle(data.id, data.angle))); + } + break; + case "kinematic/angles": + if (data.angles) { + ws.clientState.model.servos.angles = data.angles; + ws.send( + JSON.stringify({ + type: "angles", + data: ws.clientState.model.servos.angles, + }) + ); + } else { + ws.send(JSON.stringify(updateAngles(data.angles))); + } + break; + case "kinematic/bodystate": + if (data.angles) { + ws.send( + JSON.stringify({ + type: "angles", + data: updateBodyState( + ws.clientState.model, + data.angles, + data.position + ), + }) + ); + } else { + ws.send(JSON.stringify({ angles: model.servos.angles })); + } + break; + case "system/logs": + ws.send(JSON.stringify({ type: "logs", data: ws.clientState.logs })); + break; + case "system/info": + ws.send(JSON.stringify({ type: "info", data: updateSystem() })); + break; + case "system/settings": + if (data.settings) { + Object.entries(data.settings).forEach( + ([key, value]) => (ws.clientState.settings[key] = value) + ); + ws.send(JSON.stringify(ws.clientState.settings)); + } else { + ws.send( + JSON.stringify({ + type: "settings", + settings: ws.clientState.settings, + }) + ); + } + break; + case "system/stop": + ws.clientState.model.running = false; + ws.clientState.logs.push( + "[2024-02-05 19:10:00] [Warning] STOPPING SERVOS" + ); + ws.send( + JSON.stringify({ type: "log", data: ws.clientState.logs.last() }) + ); + break; + default: + ws.send(JSON.stringify({ error: "Unknown request type" })); + } +}; + wss.on("connection", (ws) => { const clientState = createNewClientState(); ws.clientState = clientState; ws.on("error", console.error); ws.on("message", (message) => { - let data = message; - try { - data = JSON.parse(message); - } catch (error) { + if (typeof message === "object") { + handleBufferMessage(ws, message); return; } - switch (data.type) { - case "subscribe": - subscribeClientToCategory(ws, data.category); - break; - case "unsubscribe": - unsubscribeClientFromCategory(ws, data.category); - break; - case "sensor/battery": - ws.send({ type: "battery", data: JSON.stringify(updateBattery()) }); - break; - case "sensor/mpu": - ws.send({ type: "battery", data: JSON.stringify(updateMpu()) }); - break; - case "sensor/distances": - ws.send(JSON.stringify(updateDistances())); - break; - case "sensor/distance": - ws.send(JSON.stringify({ distance: updateDistance(data.position) })); - break; - case "kinematic/angle": - if (data.angle && data.id) { - ws.clientState.model.servos.angles[data.id] = data.angle; - ws.send( - JSON.stringify({ - type: "angles", - data: ws.clientState.model.servos.angles, - }) - ); - } else { - ws.send(JSON.stringify(updateAngle(data.id, data.angle))); - } - break; - case "kinematic/angles": - if (data.angles) { - ws.clientState.model.servos.angles = data.angles; - ws.send( - JSON.stringify({ - type: "angles", - data: ws.clientState.model.servos.angles, - }) - ); - } else { - ws.send(JSON.stringify(updateAngles(data.angles))); - } - break; - case "kinematic/bodystate": - if (data.angles) { - ws.send( - JSON.stringify({ - type: "angles", - data: updateBodyState(ws.clientState.model, data.angles, data.position), - }) - ); - } else { - ws.send(JSON.stringify({ angles: model.servos.angles })); - } - break; - case "system/logs": - ws.send(JSON.stringify({ type: "logs", data:ws.clientState.logs })); - break; - case "system/info": - ws.send(JSON.stringify({ type: "info", data: updateSystem() })); - break; - case "system/settings": - if (data.settings) { - Object.entries(data.settings).forEach( - ([key, value]) => (ws.clientState.settings[key] = value) - ); - ws.send(JSON.stringify(ws.clientState.settings)); - } else { - ws.send(JSON.stringify({type:"settings", settings: ws.clientState.settings})); - } - break; - case "system/stop": - ws.clientState.model.running = false; - ws.clientState.logs.push("[2024-02-05 19:10:00] [Warning] STOPPING SERVOS") - ws.send(JSON.stringify({type:"log", data:ws.clientState.logs.last()})); - break; - default: - ws.send(JSON.stringify({ error: "Unknown request type" })); - } + + handleJsonMessage(ws, message); }); ws.on("close", () => {