Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Make swerve examples multiply desired module speeds by cosine of heading error (#5758)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
kully
2023-10-13 09:20:57 +03:00
committed by GitHub
parent 1c724884ca
commit 9a0aafd8ab
6 changed files with 54 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -48,9 +48,17 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
frc::Rotation2d encoderRotation{
units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
const auto state = frc::SwerveModuleState::Optimize(
referenceState, units::radian_t{m_turningEncoder.GetDistance()});
auto state =
frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
// Scale speed by cosine of angle error. This scales down movement
// perpendicular to the desired direction of travel that can occur when
// modules change directions. This results in smoother driving.
state.speed *= (state.angle - encoderRotation).Cos();
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(