Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] SwerveDriveKinematics: Add reset method (#5398)

Browse Source 
Adds a reset method where teams can pass in module headings for the kinematics object to use if it gets an all-zero ChassisSpeeds while converting ChassisSpeeds to module states. Also removes internal states array, replacing it with an internal headings array.
This commit is contained in:
Gold856
2023-06-20 01:57:55 -04:00
committed by GitHub
parent 316cd2a453
commit 50b90ceb54
5 changed files with 103 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -125,6 +125,25 @@ TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
Rotation2d fl = {0_deg};
Rotation2d fr = {90_deg};
Rotation2d bl = {180_deg};
Rotation2d br = {270_deg};
m_kinematics.ResetHeadings(fl, fr, bl, br);
auto [flMod, frMod, blMod, brMod] =
m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon);
EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, 135_deg};