From 9c5096a3c5b5ded3c18dc8669606d50a25e780ca Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Mon, 4 Mar 2024 15:56:11 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=BE=20Updates=20controller=20to=20have?=
 =?UTF-8?q?=20command?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/components/Controls.svelte | 17 ++++++------
 app/src/lib/stores/model-store.ts  |  2 +-
 mock/server.js                     | 43 +++++++++++++++++++++---------
 3 files changed, 40 insertions(+), 22 deletions(-)

diff --git a/app/src/components/Controls.svelte b/app/src/components/Controls.svelte
index 954c86b..ae52c4e 100644
--- a/app/src/components/Controls.svelte
+++ b/app/src/components/Controls.svelte
@@ -12,7 +12,7 @@
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-	let data = new Int8Array(7);
+	let data = new Int8Array($outControllerData.length);
 
 	onMount(() => {
 		left = nipplejs.create({
@@ -46,13 +46,14 @@
 	};
 
 	const updateData = () => {
-		data[0] = 0;
-		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);
+		data[0] = 1;
+		data[1] = 0;
+		data[2] = toInt8($input.left.x, -1, 1);
+		data[3] = toInt8($input.left.y, -1, 1);
+		data[4] = toInt8($input.right.x, -1, 1);
+		data[5] = toInt8($input.right.y, -1, 1);
+		data[6] = toInt8($input.height, 0, 100);
+		data[7] = toInt8($input.speed, 0, 100);
 
 		outControllerData.set(data);
 	};
diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts
index 8418e5b..427eb4c 100644
--- a/app/src/lib/stores/model-store.ts
+++ b/app/src/lib/stores/model-store.ts
@@ -14,7 +14,7 @@ export type Modes = (typeof modes)[number];
 
 export const mode: Writable<Modes> = writable('idle');
 
-export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
+export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 0, 70, 0]));
 
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },
diff --git a/mock/server.js b/mock/server.js
index 1ccdb9e..b530c82 100644
--- a/mock/server.js
+++ b/mock/server.js
@@ -209,13 +209,14 @@ const updateAngles = (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],
+    command: buffer[0],
+    stop: buffer[1],
+    lx: buffer[2],
+    ly: buffer[3],
+    rx: buffer[4],
+    ry: buffer[5],
+    h: buffer[6],
+    s: buffer[7],
   };
 };
 
@@ -276,15 +277,31 @@ const stand = (client) => {
 // https://www.hindawi.com/journals/cin/2016/9853070/
 
 const step = (model, controller, tick) => {
-  const y1 = -100 * Math.sin(-0.05 * tick) - 150;
-  const y2 = -100 * Math.sin(-0.05 * tick + Math.PI) - 150;
-  const x1 = Math.abs((tick % 120) - 60) - 60;
+  const arc_height = controller.h;
+  const speed = (controller.s + 128) / 255 / 4;
+
+  const T_stride_s = 500 / 1000;
+  const overlay = 10 / 100;
+
+  const T_stance_s = T_stride_s * (0.5 + overlay);
+  const T_swing_s = T_stride_s * (0.5 - overlay);
+
+  const x = Math.abs((tick % 200) - 100);
+
+  const y1 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick) - 150, -200),
+    -100
+  );
+  const y2 = Math.min(
+    Math.max(-arc_height * Math.sin(-speed * tick + Math.PI) - 150, -200),
+    -100
+  );
   const Lp = [
     // -50  is minimum
     [100, y1, 100, 1],
     [100, y2, -100, 1],
-    [-100, y2, 100, 1],
-    [-100, y1, -100, 1],
+    [-65, y2, 100, 1],
+    [-65, y1, -100, 1],
   ];
 
   model.servos.angles = kinematic
@@ -336,7 +353,7 @@ const handelController = (ws, buffer) => {
 };
 
 const handleBufferMessage = (ws, buffer) => {
-  if (buffer.length === 6) {
+  if (buffer.length === 8) {
     handelController(ws, buffer);
   }
 };