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] Conserve previously calculated swerve module angles when updating states for stationary ChassisSpeeds (#4208)

Browse Source 
* Calculated swerve module states now stored in a member variable
* If ChassisSpeeds(0, 0, 0) is converted to module speeds, the
previously calculated module angle will be conserved, with forward speed
set to 0
* New tests added
This commit is contained in:
Kaitlyn Kenwell
2022-05-06 11:38:20 -04:00
committed by GitHub
parent ef7ed21a9d
commit 708a4bc3bc
5 changed files with 73 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -89,6 +89,23 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::numbers::pi)};
m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};