diff --git a/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp b/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp index fe5bb9567..1b87efedc 100644 --- a/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp +++ b/zebROS_ws/src/talon_swerve_drive_controller/src/swerve_drive_controller.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -173,9 +174,18 @@ bool init(hardware_interface::RobotHW *hw, } if (!controller_nh.getParam("stopping_ff", stopping_ff_)) { - ROS_ERROR("Could not read stoping_ff in talon swerve drive controller"); + ROS_ERROR("Could not read stopping_ff in talon swerve drive controller"); return false; } + double traction_dummy; + if (controller_nh.getParam("traction_gain_percent", traction_dummy)) + { + traction_gain_percent_ = traction_dummy; + } + else { + ROS_WARN("Could not read traction_gain_percent in talon swerve drive controller"); + traction_gain_percent_ = std::nullopt; + } XmlRpc::XmlRpcValue wheel_coords; @@ -1004,6 +1014,17 @@ bool percentOutDriveModeService(std_srvs::SetBool::Request &req, std_srvs::SetBo } } +float get_limited_velocity(const ros::Time &time, size_t index) +{ + double goal_vel = speeds_angles_[index][0]; + if (!traction_gain_percent_) { return goal_vel; } + + double current_vel = latency_compensation_state_->getLatencyCompensatedValue(speed_names_[index], time); + if (fabs(goal_vel) <= fabs(current_vel)) { return goal_vel; } + return fmin(goal_vel, current_vel * (1.0 + *traction_gain_percent_)); +} + +std::optional traction_gain_percent_; // percent 0-100, amount of increase that should be allowed std::string name_; // Controller name, for debugging