Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21: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

34
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -23,9 +23,14 @@ namespace frc {
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
* Note: It is important to reset both your encoders to zero before you start
* using this class. Only reset your encoders ONCE. You should not reset your
* encoders even if you want to reset your robot's pose.
* There are two ways of tracking the robot's position on the field with this
* class: one involving using encoder velocities and the other involving encoder
* positions. It is very important that only one type of odometry is used with
* each instantiation of this class.
*
* Note: If you are using the encoder positions / distances method, it is
* important that you reset your encoders to zero before using this class. Any
* subsequent pose resets also require the encoders to be reset to zero.
*/
class DifferentialDriveOdometry {
public:
@@ -43,6 +48,9 @@ class DifferentialDriveOdometry {
/**
* Resets the robot's position on the field.
*
* If you are using the encoder distances method instead of the velocity
* method, you NEED to reset your encoders (to zero) when calling this method.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
@@ -53,6 +61,9 @@ class DifferentialDriveOdometry {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_prevLeftDistance = 0_m;
m_prevRightDistance = 0_m;
}
/**
@@ -79,6 +90,20 @@ class DifferentialDriveOdometry {
const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds);
/**
* Updates the robot position on the field using distance measurements from
* encoders. This method is more numerically accurate than using velocities to
* integrate the pose and is also advantageous for teams that are using lower
* CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
@@ -105,5 +130,8 @@ class DifferentialDriveOdometry {
units::second_t m_previousTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
units::meter_t m_prevLeftDistance = 0_m;
units::meter_t m_prevRightDistance = 0_m;
};
} // namespace frc