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

21
wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -37,3 +37,24 @@ const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
return m_pose;
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {
auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
auto deltaRightDistance = rightDistance - m_prevRightDistance;
m_prevLeftDistance = leftDistance;
m_prevRightDistance = rightDistance;
auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
auto angle = gyroAngle + m_gyroOffset;
auto newPose = m_pose.Exp(
{averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}