Skip to content

Commit

Permalink
Update throttle and steering rates in SteamJoyNodeConfig and globals.js
Browse files Browse the repository at this point in the history
  • Loading branch information
dylanzemlin committed Apr 12, 2024
1 parent 9e4ee49 commit 1b4d387
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 6 deletions.
12 changes: 7 additions & 5 deletions autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,10 @@ struct SteamJoyNodeConfig
float max_forward_speed;
bool invert_throttle;
bool invert_steering;
float throttle_rate;
float steering_rate;

NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed, invert_throttle, invert_steering)
NLOHMANN_DEFINE_TYPE_INTRUSIVE(SteamJoyNodeConfig, throttle_deadzone, steering_deadzone, forward_speed, turn_speed, max_turn_speed, max_forward_speed, invert_throttle, invert_steering, throttle_rate, steering_rate)
};

class SteamJoyNode : public SCR::Node
Expand Down Expand Up @@ -55,6 +57,8 @@ class SteamJoyNode : public SCR::Node
defaultConfig.max_forward_speed = 2.2f;
defaultConfig.invert_steering = true;
defaultConfig.invert_throttle = true;
defaultConfig.throttle_rate = 0.01f;
defaultConfig.steering_rate = 0.01f;
return defaultConfig;
}

Expand Down Expand Up @@ -114,10 +118,8 @@ class SteamJoyNode : public SCR::Node
}

// Generate a forward/angular velocity command that ramps up/down smoothly
const float throttleRate = 0.03;
const float steeringRate = 0.01;
current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, throttleRate * (is_working ? 1 : 1.8));
current_steering = lerp(current_steering, target_steering * config.turn_speed, steeringRate * (is_working ? 1 : 1.8));
current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, config.throttle_rate * (is_working ? 1 : 2.4));
current_steering = lerp(current_steering, target_steering * config.turn_speed, config.steering_rate * (is_working ? 1 : 1.8));

autonav_msgs::msg::MotorInput input;
// input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed);
Expand Down
4 changes: 3 additions & 1 deletion display/scripts/globals.js
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,9 @@ var addressKeys = {
"max_forward_speed": "float",
"max_turn_speed": "float",
"invert_steering": "bool",
"invert_throttle": "bool"
"invert_throttle": "bool",
"throttle_rate": "float",
"steering_rate": "float"
},

"autonav_nav_astar": {
Expand Down

0 comments on commit 1b4d387

Please sign in to comment.