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

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

@@ -119,6 +119,28 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
void testResetWheelAngle() {
Rotation2d fl = new Rotation2d(0);
Rotation2d fr = new Rotation2d(Math.PI / 2);
Rotation2d bl = new Rotation2d(Math.PI);
Rotation2d br = new Rotation2d(3 * Math.PI / 2);
m_kinematics.resetHeadings(fl, fr, bl, br);
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(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);