@ -693,7 +693,7 @@ class GamepadManager : public InputInterface {
double final_speed = planner_use_movement_speed_ ;
double final_speed = planner_use_movement_speed_ ;
double final_height = planner_use_height_ ;
double final_height = planner_use_height_ ;
// LOCKED TO SLOW_WALK for testing — speed 0.0-0.8 m/s
// LOCKED TO SLOW_WALK for testing — speed 0.2-0.8 m/s (planner trained range)
// Proportional to stick magnitude with acceleration rate limiting.
// Proportional to stick magnitude with acceleration rate limiting.
if ( planner_stick_magnitude_ < dead_zone_ ) {
if ( planner_stick_magnitude_ < dead_zone_ ) {
// Stick in dead zone — IDLE
// Stick in dead zone — IDLE
@ -701,25 +701,25 @@ class GamepadManager : public InputInterface {
final_movement = { 0.0f , 0.0f , 0.0f } ;
final_movement = { 0.0f , 0.0f , 0.0f } ;
final_speed = - 1.0f ;
final_speed = - 1.0f ;
final_height = - 1.0f ;
final_height = - 1.0f ;
smoothed_speed_ = 0.0 ; // Reset ramp when idl e
smoothed_speed_ = 0.2 ; // Reset to planner minimum so ramp starts ther e
} else if ( planner_use_movement_mode_ = = static_cast < int > ( LocomotionMode : : CRAWLING ) | |
} else if ( planner_use_movement_mode_ = = static_cast < int > ( LocomotionMode : : CRAWLING ) | |
planner_use_movement_mode_ = = static_cast < int > ( LocomotionMode : : IDEL_KNEEL_TWO_LEGS ) ) {
planner_use_movement_mode_ = = static_cast < int > ( LocomotionMode : : IDEL_KNEEL_TWO_LEGS ) ) {
// Keep crawl/kneel modes as-is
// Keep crawl/kneel modes as-is
double normalized = std : : min ( ( planner_stick_magnitude_ - dead_zone_ ) / ( 1.0 - dead_zone_ ) , 1.0 ) ;
double normalized = std : : min ( ( planner_stick_magnitude_ - dead_zone_ ) / ( 1.0 - dead_zone_ ) , 1.0 ) ;
final_speed = 0.3 + normalized * ( 1.2 - 0.3 ) ;
final_speed = 0.3 + normalized * ( 1.2 - 0.3 ) ;
} else {
} else {
// Everything else → SLOW_WALK, 0.0-0.8 m/s
// Everything else → SLOW_WALK, 0.2-0.8 m/s (planner trained range)
double normalized = std : : min ( ( planner_stick_magnitude_ - dead_zone_ ) / ( 1.0 - dead_zone_ ) , 1.0 ) ;
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
double target_speed = 0.2 + normalized * ( 0.8 - 0.2 ) ; // 0.2 min, 0.8 max
// Rate-limit acceleration only (decel is instant)
// Rate-limit acceleration only (decel is instant)
constexpr double max_accel_per_frame = 0.002 ; // 4s ramp 0→0.8 (100Hz input loop)
constexpr double max_accel_per_frame = 0.003 ; // ~2s ramp 0.2→0.8 at 100Hz
if ( target_speed > smoothed_speed_ ) {
if ( target_speed > smoothed_speed_ ) {
smoothed_speed_ = std : : min ( target_speed , smoothed_speed_ + max_accel_per_frame ) ;
smoothed_speed_ = std : : min ( target_speed , smoothed_speed_ + max_accel_per_frame ) ;
} else {
} else {
smoothed_speed_ = target_speed ; // Instant decel
smoothed_speed_ = target_speed ; // Instant decel
}
}
final_mode = static_cast < int > ( LocomotionMode : : SLOW_WALK ) ;
final_mode = static_cast < int > ( LocomotionMode : : SLOW_WALK ) ;
final_speed = smoothed_speed_ ;
final_speed = std : : max ( s moothed_speed_ , 0.2 ) ; // Never below planner minimum
}
}
// Emergency stop resets to idle
// Emergency stop resets to idle