From c89ef9de2646c2ba1a6ae4f363f1192fbdcc5b13 Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Sun, 22 Feb 2026 14:21:42 -0600
Subject: [PATCH] Lock to SLOW_WALK mode with stock speed range (0.2-0.8 m/s)
 for testing
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Ignores X/Y mode selection — all stick input maps to SLOW_WALK with
proportional speed 0.2-0.8 m/s (matching stock Gamepad range).
Crawl/kneel modes preserved.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp       | 59 +++++--------------
 1 file changed, 15 insertions(+), 44 deletions(-)

diff --git a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
index 4cfe475..0fc517a 100644
--- a/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
+++ b/gear_sonic_deploy/src/g1/g1_deploy_onnx_ref/include/input_interface/gamepad_manager.hpp
@@ -668,53 +668,24 @@ class GamepadManager : public InputInterface {
         double final_speed = planner_use_movement_speed_;
         double final_height = planner_use_height_;
 
-        // Proportional speed with auto locomotion mode switching
-        // X (WALK macro): SLOW_WALK(0.1-0.8) → WALK(0.8-2.0) based on stick magnitude
-        // Y (RUN macro):  WALK(0.8-2.5) → RUN(2.5-4.5) based on stick magnitude
+        // LOCKED TO SLOW_WALK for testing — stock speed range 0.2-0.8 m/s
+        // Proportional to stick magnitude. X/Y button selection ignored.
         if (planner_stick_magnitude_ < dead_zone_) {
-          // Stick in dead zone — IDLE for walk/run, zero-movement for crawl/kneel
-          if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::WALK) ||
-              planner_use_movement_mode_ == static_cast<int>(LocomotionMode::RUN)) {
-            final_mode = static_cast<int>(LocomotionMode::IDLE);
-            final_movement = {0.0f, 0.0f, 0.0f};
-            final_speed = -1.0f;
-            final_height = -1.0f;
-          } else {
-            final_movement = {0.0f, 0.0f, 0.0f};
-            final_speed = 0.0f;
-            final_height = 0.4f;
-          }
-        } else if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::WALK)) {
-          // WALK macro: SLOW_WALK (0.1-0.8) → WALK (0.8-2.0)
-          double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
-          if (normalized < 0.5) {
-            // Lower half: SLOW_WALK gait, speed 0.1 to 0.8
-            final_mode = static_cast<int>(LocomotionMode::SLOW_WALK);
-            double t = normalized / 0.5;
-            final_speed = 0.1 + t * (0.8 - 0.1);
-          } else {
-            // Upper half: WALK gait, speed 0.8 to 2.0
-            final_mode = static_cast<int>(LocomotionMode::WALK);
-            double t = (normalized - 0.5) / 0.5;
-            final_speed = 0.8 + t * (2.0 - 0.8);
-          }
-        } else if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::RUN)) {
-          // RUN macro: WALK (0.8-2.5) → RUN (2.5-4.5)
-          double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
-          if (normalized < 0.5) {
-            // Lower half: WALK gait, speed 0.8 to 2.5
-            final_mode = static_cast<int>(LocomotionMode::WALK);
-            double t = normalized / 0.5;
-            final_speed = 0.8 + t * (2.5 - 0.8);
-          } else {
-            // Upper half: RUN gait, speed 2.5 to 4.5
-            final_mode = static_cast<int>(LocomotionMode::RUN);
-            double t = (normalized - 0.5) / 0.5;
-            final_speed = 2.5 + t * (4.5 - 2.5);
-          }
-        } else if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::CRAWLING)) {
+          // Stick in dead zone — IDLE
+          final_mode = static_cast<int>(LocomotionMode::IDLE);
+          final_movement = {0.0f, 0.0f, 0.0f};
+          final_speed = -1.0f;
+          final_height = -1.0f;
+        } else if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::CRAWLING) ||
+                   planner_use_movement_mode_ == static_cast<int>(LocomotionMode::IDEL_KNEEL_TWO_LEGS)) {
+          // Keep crawl/kneel modes as-is
           double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
           final_speed = 0.3 + normalized * (1.2 - 0.3);
+        } else {
+          // Everything else → SLOW_WALK, stock range 0.2-0.8
+          double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
+          final_mode = static_cast<int>(LocomotionMode::SLOW_WALK);
+          final_speed = 0.2 + normalized * (0.8 - 0.2);
         }
 
         // Emergency stop resets to idle