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

[wpimath] Add support for swerve joystick normalization (#4516)

Browse Source
This commit is contained in:
Drew Williams
2022-11-17 13:53:49 -05:00
committed by GitHub
parent ce4c45df13
commit 2e88a496c2
5 changed files with 148 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -351,4 +351,24 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
@Test
void testDesaturateSmooth() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(
arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
}

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

@@ -256,3 +256,21 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
&arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}