Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

19
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -77,6 +77,25 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testConserveWheelAngle() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
m_kinematics.toSwerveModuleStates(speeds);
var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
// Robot is stationary, but module angles are preserved.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
() -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);