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

📜 Makes data sync more seamless

Browse Source
This commit is contained in:
Rune Harlyk
2024-02-23 13:43:23 +01:00
parent 04c3603254
commit 99a185ac82
9 changed files with 228 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/App.svelte
+2 -1
View File
@@ -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');
app/src/components/Controls.svelte
+9 -9
View File
@@ -1,7 +1,7 @@
<script lang="ts">
import nipplejs from 'nipplejs';
import { onMount } from 'svelte';
import { throttler, toUint8 } from '$lib/utilities';
import { throttler, toInt8 } from '$lib/utilities';
import socketService from '$lib/services/socket-service';
import { emulateModel, input, outControllerData } from '$lib/store';
@@ -13,7 +13,7 @@
let mode = 'rest'; // 'rest' | 'stand' | 'stand+' | 'walk'
let data = new Uint8Array(6);
let data = new Int8Array(6);
onMount(() => {
left = nipplejs.create({
@@ -67,16 +67,16 @@
const updateData = () => {
data[0] = 0;
data[1] = toUint8($input.left.x, -1, 1);
data[2] = toUint8($input.left.y, -1, 1);
data[3] = toUint8($input.right.x, -1, 1);
data[4] = toUint8($input.right.y, -1, 1);
data[5] = toUint8($input.height, 0, 100);
data[6] = toUint8($input.speed, 0, 100);
data[1] = toInt8($input.left.x, -1, 1);
data[2] = toInt8($input.left.y, -1, 1);
data[3] = toInt8($input.right.x, -1, 1);
data[4] = toInt8($input.right.y, -1, 1);
data[5] = toInt8($input.height, 0, 100);
data[6] = toInt8($input.speed, 0, 100);
outControllerData.set(data);
if (!$emulateModel) socketService.send(data);
if (!$emulateModel) socketService.send(data);
};
</script>
app/src/components/Views/Model.svelte
+2 -14
View File
@@ -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(() => {
app/src/lib/services/socket-service.ts
+5
View File
@@ -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<WebsocketOutData>, type?: string) {
store.subscribe((data) => this.send(type ? JSON.stringify({ type, data }) : data));
}
private handleConnected(): void {
isConnected.set(true);
}
app/src/lib/store.ts
+1 -1
View File
@@ -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', []);
app/src/lib/utilities/math-utilities.ts
+6
View File
@@ -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;
};
app/test/specs/number-convert.spec.ts
+46
View File
@@ -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);
});
});
app/test/specs/toUint8.spec.ts
-24
View File
@@ -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);
});
});
mock/server.js
+157 -85
View File
@@ -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", () => {