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

Add encoder distance overload to DifferentialDriveOdometry (#2096)

Browse Source 
Also force encoders to be reset to zero on pose reset.
This commit is contained in:
Prateek Machiraju
2019-11-19 12:56:34 -05:00
committed by Peter Johnson
parent 845aba33fe
commit 45201d15fc
5 changed files with 123 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -57,3 +57,15 @@ TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) {
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}
TEST(DifferentialDriveOdometry, EncoderDistances) {
DifferentialDriveKinematics kinematics{0.381_m * 2};
DifferentialDriveOdometry odometry{kinematics, Rotation2d(45_deg)};
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::math::pi));
EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
}