📜 Makes data sync more seamless

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');
 

app/src/components/Controls.svelte

@@ -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[1] = toInt8($input.left.x, -1, 1);
-		data[2] = toUint8($input.left.y, -1, 1);
+		data[2] = toInt8($input.left.y, -1, 1);
-		data[3] = toUint8($input.right.x, -1, 1);
+		data[3] = toInt8($input.right.x, -1, 1);
-		data[4] = toUint8($input.right.y, -1, 1);
+		data[4] = toInt8($input.right.y, -1, 1);
-		data[5] = toUint8($input.height, 0, 100);
+		data[5] = toInt8($input.height, 0, 100);
-		data[6] = toUint8($input.speed, 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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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;
+    if (typeof message === "object") {
-    try {
+      handleBufferMessage(ws, message);
-      data = JSON.parse(message);
-    } catch (error) {
       return;
     }
-    switch (data.type) {
+
-      case "subscribe":
+    handleJsonMessage(ws, message);
-        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" }));
-    }
   });
 
   ws.on("close", () => {