added CosineScaling c++ tests

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

Hide file tree

Showing 5 changed files with 92 additions and 10 deletions.
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -46,8 +46,7 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 

diff --git a/...ibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/...ibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -53,8 +53,7 @@ frc::SwerveModulePosition SwerveModule::GetPosition() {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 

diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
@@ -46,8 +46,7 @@ frc::SwerveModulePosition SwerveModule::GetPosition() const {
           units::radian_t{m_turningEncoder.GetDistance()}};
 }
 
-void SwerveModule::SetDesiredState(
-    frc::SwerveModuleState& referenceState) {
+void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 

diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -64,16 +64,15 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
                                     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.
+   * 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
 

diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -41,6 +41,92 @@ TEST(SwerveModuleStateTest, NoOptimize) {
   EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
 }
 
+TEST(SwerveModuleStateTest, CosineScaling) {
+  frc::Rotation2d angleA{0_deg};
+  frc::SwerveModuleState refA{2_mps, 45_deg};
+  refA.cosineScale(angleA);
+
+  EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleB{45_deg};
+  frc::SwerveModuleState refB{2_mps, 45_deg};
+  refB.cosineScale(angleB);
+
+  EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleC{-45_deg};
+  frc::SwerveModuleState refC{2_mps, 45_deg};
+  refC.cosineScale(angleC);
+
+  EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleD{135_deg};
+  frc::SwerveModuleState refD{2_mps, 45_deg};
+  refD.cosineScale(angleD);
+
+  EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleE{-135_deg};
+  frc::SwerveModuleState refE{2_mps, 45_deg};
+  refE.cosineScale(angleE);
+
+  EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleF{180_deg};
+  frc::SwerveModuleState refF{2_mps, 45_deg};
+  refF.cosineScale(angleF);
+
+  EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleG{0_deg};
+  frc::SwerveModuleState refG{-2_mps, 45_deg};
+  refG.cosineScale(angleG);
+
+  EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleH{45_deg};
+  frc::SwerveModuleState refH{-2_mps, 45_deg};
+  refH.cosineScale(angleH);
+
+  EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleI{-45_deg};
+  frc::SwerveModuleState refI{-2_mps, 45_deg};
+  refI.cosineScale(angleI);
+
+  EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleJ{135_deg};
+  frc::SwerveModuleState refJ{-2_mps, 45_deg};
+  refJ.cosineScale(angleJ);
+
+  EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleK{-135_deg};
+  frc::SwerveModuleState refK{-2_mps, 45_deg};
+  refK.cosineScale(angleK);
+
+  EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
+
+  frc::Rotation2d angleL{180_deg};
+  frc::SwerveModuleState refL{-2_mps, 45_deg};
+  refL.cosineScale(angleL);
+
+  EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
+}
+
 TEST(SwerveModuleStateTest, Equality) {
   frc::SwerveModuleState state1{2_mps, 90_deg};
   frc::SwerveModuleState state2{2_mps, 90_deg};