Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Remove encoder velocities methods in DifferentialDriveOdometry (#2147)

Browse Source 
It doesn't make sense to continue to provide a less accurate method of performing odometry
when a more accurate method using distances exists.

This also removes the need to pass DifferentialDriveKinematics to the constructor.
This commit is contained in:
Prateek Machiraju
2019-12-01 02:10:29 -05:00
committed by Peter Johnson
parent b8c1024261
commit 5b73c17f25
10 changed files with 41 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,34 +10,12 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
const Rotation2d& gyroAngle, const Pose2d& initialPose)
: m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
return m_pose;
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance) {