Check if SwerveDriveKinematics<5> works

KangarooKoala · Nov 29, 2023 · e2a26a9 · e2a26a9
1 parent 5f71828
commit e2a26a9
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -24,6 +24,16 @@ class SwerveDriveKinematicsTest : public ::testing::Test {
   SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
 };
 
+TEST_F(SwerveDriveKinematicsTest, Desaturate5Wheels) {
+  SwerveModuleState state1{1.0_mps, 0_deg};
+  SwerveModuleState state2{1.0_mps, 0_deg};
+  SwerveModuleState state3{1.0_mps, 0_deg};
+  SwerveModuleState state4{1.0_mps, 0_deg};
+  SwerveModuleState state5{1.0_mps, 0_deg};
+  wpi::array<SwerveModuleState, 5> arr{state1, state2, state3, state4, state5};
+  SwerveDriveKinematics<5>::DesaturateWheelSpeeds(&arr, 1.0_mps);
+}
+
 TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
   ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};