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 cosineScale method to SwerveModuleState and instance optimize (#7114)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-09-30 15:23:30 -04:00
committed by GitHub
parent fde264b041
commit fe80d72fba
13 changed files with 311 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -46,29 +46,29 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
frc::Rotation2d encoderRotation{
units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
auto state =
frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
referenceState.Optimize(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();
referenceState.CosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), state.speed.value());
m_driveEncoder.GetRate(), referenceState.speed.value());
const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
const auto driveFeedforward =
m_driveFeedforward.Calculate(referenceState.speed);
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
units::radian_t{m_turningEncoder.GetDistance()},
referenceState.angle.Radians());
const auto turnFeedforward = m_turnFeedforward.Calculate(
m_turningPIDController.GetSetpoint().velocity);

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
View File

@@ -25,7 +25,7 @@ class SwerveModule {
int turningEncoderChannelA, int turningEncoderChannelB);
frc::SwerveModuleState GetState() const;
frc::SwerveModulePosition GetPosition() const;
void SetDesiredState(const frc::SwerveModuleState& state);
void SetDesiredState(frc::SwerveModuleState& state);
private:
static constexpr double kWheelRadius = 0.0508;

13
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
View File

@@ -53,27 +53,26 @@ frc::SwerveModulePosition SwerveModule::GetPosition() {
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
frc::Rotation2d encoderRotation{
units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
auto state =
frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
referenceState.Optimize(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();
referenceState.CosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), state.speed.value());
m_driveEncoder.GetRate(), referenceState.speed.value());
// Calculate the turning motor output from the turning PID controller.
auto turnOutput = m_turningPIDController.Calculate(
units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
units::radian_t{m_turningEncoder.GetDistance()},
referenceState.angle.Radians());
// Set the motor outputs.
m_driveMotor.Set(driveOutput);

2
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
View File

@@ -27,7 +27,7 @@ class SwerveModule {
frc::SwerveModulePosition GetPosition();
void SetDesiredState(const frc::SwerveModuleState& state);
void SetDesiredState(frc::SwerveModuleState& state);
void ResetEncoders();

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

@@ -46,29 +46,29 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
frc::Rotation2d encoderRotation{
units::radian_t{m_turningEncoder.GetDistance()}};
// Optimize the reference state to avoid spinning further than 90 degrees
auto state =
frc::SwerveModuleState::Optimize(referenceState, encoderRotation);
referenceState.Optimize(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();
referenceState.CosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
const auto driveOutput = m_drivePIDController.Calculate(
m_driveEncoder.GetRate(), state.speed.value());
m_driveEncoder.GetRate(), referenceState.speed.value());
const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
const auto driveFeedforward =
m_driveFeedforward.Calculate(referenceState.speed);
// Calculate the turning motor output from the turning PID controller.
const auto turnOutput = m_turningPIDController.Calculate(
units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
units::radian_t{m_turningEncoder.GetDistance()},
referenceState.angle.Radians());
const auto turnFeedforward = m_turnFeedforward.Calculate(
m_turningPIDController.GetSetpoint().velocity);

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h
View File

@@ -25,7 +25,7 @@ class SwerveModule {
int turningEncoderChannelA, int turningEncoderChannelB);
frc::SwerveModuleState GetState() const;
frc::SwerveModulePosition GetPosition() const;
void SetDesiredState(const frc::SwerveModuleState& state);
void SetDesiredState(frc::SwerveModuleState& state);
private:
static constexpr auto kWheelRadius = 0.0508_m;