From 4c05ba695bff2ce9930a9577a00c443b8fa27449 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 3 Aug 2024 18:13:31 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=93=8F=20Adds=20step=20length=20slider?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/models.ts                      | 1 +
 app/src/lib/stores/model-store.ts          | 3 ++-
 app/src/routes/controller/Controls.svelte  | 9 ++++++---
 esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 8 ++++----
 esp32/lib/ESP32-sveltekit/MotionService.h  | 5 +++--
 5 files changed, 16 insertions(+), 10 deletions(-)

diff --git a/app/src/lib/models.ts b/app/src/lib/models.ts
index 647f6e0..5194c55 100644
--- a/app/src/lib/models.ts
+++ b/app/src/lib/models.ts
@@ -5,6 +5,7 @@ export interface ControllerInput {
 	right: vector;
 	height: number;
 	speed: number;
+	s1: number;
 }
 
 export type GithubRelease = {
diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts
index 5114471..7061fd4 100644
--- a/app/src/lib/stores/model-store.ts
+++ b/app/src/lib/stores/model-store.ts
@@ -32,5 +32,6 @@ export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },
 	right: { x: 0, y: 0 },
 	height: 50,
-	speed: 50
+	speed: 50,
+	s1: 50
 });
diff --git a/app/src/routes/controller/Controls.svelte b/app/src/routes/controller/Controls.svelte
index dd414d6..5f547a0 100644
--- a/app/src/routes/controller/Controls.svelte
+++ b/app/src/routes/controller/Controls.svelte
@@ -10,7 +10,7 @@
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-	let data = new Array(7);
+	let data = new Array(8);
 
 	onMount(() => {
 		left = nipplejs.create({
@@ -51,6 +51,7 @@
 		data[4] = toInt8($input.right.y, -1, 1);
 		data[5] = toInt8($input.height, 0, 100);
 		data[6] = toInt8($input.speed, 0, 100);
+		data[7] = toInt8($input.s1, 0, 100);
 
 		outControllerData.set(data);
 	};
@@ -67,7 +68,7 @@
 		throttle.throttle(updateData, throttle_timing);
 	};
 
-    const handleRange = (event:Event, key: 'speed' | 'height') => {
+    const handleRange = (event:Event, key: 'speed' | 'height' | 's1') => {
         const value:number = event.target?.value
         
         input.update((inputData) => {
@@ -107,7 +108,9 @@
             </button>
 		{/each}
         <div>
-            {#if $mode === ModesEnum.Walk}
+            {#if $mode === ModesEnum.Walk || $mode === ModesEnum.Crawl}
+                <label for="s1">S1</label>
+                <input type="range" name="s1" min="0" max="100" on:input={(e) => handleRange(e, 's1')} class="range range-sm" />
                 <label for="speed">Speed</label>
                 <input type="range" name="speed" min="0" max="100" on:input={(e) => handleRange(e, 'speed')} class="range range-sm" />
             {/if}
diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
index e82864b..fec9860 100644
--- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h
@@ -10,7 +10,7 @@ struct gait_state_t {
 
 struct ControllerCommand {
     int stop;
-    float lx, ly, rx, ry, h, s;
+    float lx, ly, rx, ry, h, s, s1;
 };
 
 class GaitState {
@@ -140,9 +140,9 @@ class PhaseGaitState : public GaitState {
 
     gait_state_t mapCommand(ControllerCommand command) {
         gait_state_t state;
-        state.step_height = 0.4f + std::fabs(command.ry / 128);
-        state.step_x = command.ly / 128;
-        state.step_z = -command.lx / 128;
+        state.step_height = (command.s1 / 128 + 1) / 2;
+        state.step_x = command.ly / 128 * 2;
+        state.step_z = -command.lx / 128 * 2;
         state.step_velocity = command.s / 128 + 1;
         state.step_angle = 0;
         return state;
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index c13d0f6..7d769d5 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -72,8 +72,9 @@ class MotionService {
         command.ry = array[4];
         command.h = array[5];
         command.s = array[6];
+        command.s1 = array[7];
 
-        body_state.ym = (command.h + 127.f) * 0.75f / 100;
+        body_state.ym = (command.h + 127.f) * 0.35f / 100;
 
         switch (motionState) {
             case MOTION_STATE::STAND: {
@@ -154,7 +155,7 @@ class MotionService {
     TaskManager *_taskManager;
     ServoController *_servoController;
     Kinematics kinematics;
-    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0};
+    ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
 
     friend class GaitState;