From 4dfd008277660af5d2f11ec43cea72ef26083e75 Mon Sep 17 00:00:00 2001
From: Joe DiPrima <epilectrik@gmail.com>
Date: Sun, 22 Feb 2026 20:14:23 -0600
Subject: [PATCH] Add speed ramp-up and rescale speed range to start at 0

- Rescale SLOW_WALK speed from [0.2, 0.8] to [0.0, 0.8] so dead zone
  edge produces zero speed instead of jumping to 0.2 m/s
- Add acceleration rate limiter (0.01/frame = 0.4s ramp at 200Hz)
  that smooths speed increases but allows instant deceleration
- Fixes exaggerated first steps caused by instant IDLE to 0.2 m/s jump

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
---
 .../input_interface/gamepad_manager.hpp        | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 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 b4b17de..116c2b1 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
@@ -690,24 +690,33 @@ class GamepadManager : public InputInterface {
         double final_speed = planner_use_movement_speed_;
         double final_height = planner_use_height_;
 
-        // LOCKED TO SLOW_WALK for testing — stock speed range 0.2-0.8 m/s
-        // Proportional to stick magnitude. X/Y button selection ignored.
+        // LOCKED TO SLOW_WALK for testing — speed 0.0-0.8 m/s
+        // Proportional to stick magnitude with acceleration rate limiting.
         if (planner_stick_magnitude_ < dead_zone_) {
           // 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;
+          smoothed_speed_ = 0.0;  // Reset ramp when idle
         } 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
+          // Everything else → SLOW_WALK, 0.0-0.8 m/s
           double normalized = std::min((planner_stick_magnitude_ - dead_zone_) / (1.0 - dead_zone_), 1.0);
+          double target_speed = normalized * 0.8;  // 0.0 at dead zone edge, 0.8 at full stick
+          // Rate-limit acceleration only (decel is instant)
+          constexpr double max_accel_per_frame = 0.01;  // ~0.4s ramp 0→0.8 at 200Hz
+          if (target_speed > smoothed_speed_) {
+            smoothed_speed_ = std::min(target_speed, smoothed_speed_ + max_accel_per_frame);
+          } else {
+            smoothed_speed_ = target_speed;  // Instant decel
+          }
           final_mode = static_cast<int>(LocomotionMode::SLOW_WALK);
-          final_speed = 0.2 + normalized * (0.8 - 0.2);
+          final_speed = smoothed_speed_;
         }
 
         // Emergency stop resets to idle
@@ -811,6 +820,7 @@ class GamepadManager : public InputInterface {
     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.
+    double smoothed_speed_ = 0.0;                ///< Rate-limited speed output (smooths acceleration only).
     
     /// Timestamp when KNEEL_TWO_LEGS mode was entered; used to auto-transition
     /// to CRAWLING mode after a 2-second delay.