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

53
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
View File

@@ -21,9 +21,14 @@ import edu.wpi.first.wpilibj.geometry.Twist2d;
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
* <p>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.
* <p>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.
*
* <p>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.
*/
public class DifferentialDriveOdometry {
private final DifferentialDriveKinematics m_kinematics;
@@ -33,6 +38,9 @@ public class DifferentialDriveOdometry {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private double m_prevLeftDistance;
private double m_prevRightDistance;
/**
* Constructs a DifferentialDriveOdometry object.
*
@@ -59,8 +67,10 @@ public class DifferentialDriveOdometry {
}
/**
* Resets the robot's position on the field. Do NOT zero your encoders if you
* call this function at any other time except initialization.
* Resets the robot's position on the field.
*
* <p>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.
*
* <p>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.
@@ -72,6 +82,9 @@ public class DifferentialDriveOdometry {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_prevLeftDistance = 0.0;
m_prevRightDistance = 0.0;
}
/**
@@ -115,6 +128,36 @@ public class DifferentialDriveOdometry {
return m_poseMeters;
}
/**
* 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 leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
double rightDistanceMeters) {
double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
var angle = gyroAngle.plus(m_gyroOffset);
var newPose = m_poseMeters.exp(
new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates the