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

[wpimath] Add optimize() to SwerveModuleState (#3065)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-10 22:49:46 -08:00
committed by GitHub
parent fb99910c23
commit e955037980
9 changed files with 79 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
View File

@@ -32,7 +32,12 @@ frc::SwerveModuleState SwerveModule::GetState() const {
frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
}
void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
// Optimize the reference state to avoid spinning further than 90 degrees
const auto state = frc::SwerveModuleState::Optimize(
referenceState, units::radian_t(m_turningEncoder.Get()));
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), state.speed.to<double>());