From e889c7fa061c15039eb55d52d8f2f41dad68d0db Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Sun, 22 Feb 2026 08:41:02 -0600
Subject: [PATCH] Add proportional analog stick speed control to GamepadManager

Left stick magnitude now maps to movement speed instead of binary on/off.
Removed 30-degree angle binning for smooth continuous direction control.
WALK: 0.3-1.5 m/s, RUN: 2.0-4.5 m/s, CRAWL: 0.3-1.2 m/s proportional
to stick deflection. Dead zone behavior unchanged (IDLE when released).

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp       | 47 ++++++++++++-------
 1 file changed, 31 insertions(+), 16 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 7a06341..62fb01d 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
@@ -484,17 +484,19 @@ class GamepadManager : public InputInterface {
         }
       }
 
-      if (std::abs(lx_) > dead_zone_ || std::abs(ly_) > dead_zone_) {
-        // Bin-smooth the angle to nearest π/6 (30 degree) increment
-        double raw_angle = atan2(ly_, lx_);
-        double bin_size = M_PI / 6.0;
-        double binned_angle = std::round(raw_angle / bin_size) * bin_size;
-        
-        planner_moving_direction_ = binned_angle - M_PI/2 + planner_facing_angle_;
-        if constexpr (DEBUG_LOGGING) {
-          std::cout << "[GamepadManager DEBUG] Left stick - Raw angle: " << raw_angle 
-                    << " rad, Binned angle: " << binned_angle 
-                    << " rad, Moving direction: " << planner_moving_direction_ << " rad" << std::endl;
+      {
+        double mag = std::sqrt(lx_ * lx_ + ly_ * ly_);
+        if (mag > dead_zone_) {
+          double raw_angle = atan2(ly_, lx_);
+          planner_moving_direction_ = raw_angle - M_PI/2 + planner_facing_angle_;
+          planner_stick_magnitude_ = std::min(static_cast<double>(mag), 1.0);
+          if constexpr (DEBUG_LOGGING) {
+            std::cout << "[GamepadManager DEBUG] Left stick - Angle: " << raw_angle
+                      << " rad, Magnitude: " << planner_stick_magnitude_
+                      << ", Moving direction: " << planner_moving_direction_ << " rad" << std::endl;
+          }
+        } else {
+          planner_stick_magnitude_ = 0.0;
         }
       }
     }
@@ -648,21 +650,33 @@ class GamepadManager : public InputInterface {
         double final_speed = planner_use_movement_speed_;
         double final_height = planner_use_height_;
 
-        // If left sticks in dead zone, go to idle only for walk/run modes, stay in place for crawl/kneel
-        if (std::abs(lx_) < dead_zone_ && std::abs(ly_) < dead_zone_) {
-          // If in WALK or RUN mode, go to IDLE
-          if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::WALK) || 
+        // Proportional speed from stick magnitude (replaces binary IDLE gate)
+        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 {
+          } else {
             // Keep current mode, but zero out movement
             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) ||
+                   planner_use_movement_mode_ == static_cast<int>(LocomotionMode::RUN)) {
+          // Proportional speed from stick magnitude
+          double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
+          if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::WALK)) {
+            final_speed = 0.3 + normalized * (1.5 - 0.3);   // 0.3 to 1.5 m/s
+          } else { // RUN
+            final_speed = 2.0 + normalized * (4.5 - 2.0);   // 2.0 to 4.5 m/s
+          }
+        } else if (planner_use_movement_mode_ == static_cast<int>(LocomotionMode::CRAWLING)) {
+          double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
+          final_speed = 0.3 + normalized * (1.2 - 0.3);     // 0.3 to 1.2 m/s
         }
 
         // Emergency stop resets to idle
@@ -759,6 +773,7 @@ class GamepadManager : public InputInterface {
     double planner_use_height_ = -1.0;            ///< Desired body height (−1 = mode default).
     double planner_facing_angle_ = 0.0;           ///< Accumulated facing direction (radians).
     double planner_moving_direction_ = 0.0;       ///< Current movement direction (radians).
+    double planner_stick_magnitude_ = 0.0;       ///< Left stick magnitude (0-1), used for proportional speed.
     
     /// Timestamp when KNEEL_TWO_LEGS mode was entered; used to auto-transition
     /// to CRAWLING mode after a 2-second delay.