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] Fix MecanumDriveKinematics (#3266)

Browse Source
This commit is contained in:
Tyler Veness
2021-04-30 15:50:16 -07:00
committed by GitHub
parent ff52f207cc
commit 365f5449ca
6 changed files with 84 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -38,7 +38,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}
@@ -50,8 +50,8 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
}
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}