added c++ CosineScaling method

wpilibsuite · Sep 21, 2024 · 3df9817 · 3df9817
1 parent 2f476e6
commit 3df9817
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 3 deletions.
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -57,7 +57,7 @@ void SwerveModule::SetDesiredState(
   // 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.
-  referenceState.speed *= (referenceState.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(

diff --git a/...ibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/...ibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -64,7 +64,7 @@ void SwerveModule::SetDesiredState(
   // 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.
-  referenceState.speed *= (referenceState.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
@@ -57,7 +57,7 @@ void SwerveModule::SetDesiredState(
   // 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.
-  referenceState.speed *= (referenceState.angle - encoderRotation).Cos();
+  referenceState.CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -62,6 +62,18 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
   [[deprecated("Use instance method instead.")]]
   static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
                                     const Rotation2d& currentAngle);
+
+  /**
+   * Scales 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.
+   *
+   * @param currentAngle The current module angle.
+   */
+  void CosineScale(const Rotation2d& currentAngle) {
+    speed *= (angle - currentAngle).Cos();
+  }
+
 };
 }  // namespace frc