@ -668,53 +668,24 @@ 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_ ;
// 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_ ) {
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 ) ) {
// Stick in dead zone — IDLE
final_mode = static_cast < int > ( LocomotionMode : : IDLE ) ;
final_mode = static_cast < int > ( LocomotionMode : : IDLE ) ;
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 ;
} 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)
} 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 ) ;
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 ) ;
final_speed = 0.3 + normalized * ( 1.2 - 0.3 ) ;
} else {
} 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)
// 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 ) ;
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 ) ) {
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_mode = static_cast < int > ( LocomotionMode : : SLOW_WALK ) ;
final_speed = 0.2 + normalized * ( 0.8 - 0.2 ) ;
}
}
// Emergency stop resets to idle
// Emergency stop resets to idle