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

[wpimath] Rework odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -33,42 +33,59 @@ public class DifferentialDriveOdometry {
* Constructs a DifferentialDriveOdometry object.
*
* @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.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
public DifferentialDriveOdometry(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
/**
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
* Constructs a DifferentialDriveOdometry object.
*
* @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.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle) {
this(gyroAngle, new Pose2d());
public DifferentialDriveOdometry(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>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.
*
* @param poseMeters The position on the field that your robot is at.
* @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.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_prevLeftDistance = 0.0;
m_prevRightDistance = 0.0;
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -70,12 +70,12 @@ public class MecanumDriveOdometry {
* <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.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -77,12 +77,12 @@ public class SwerveDriveOdometry {
*
* <p>Similarly, module positions do not need to be reset in user code.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param modulePositions The wheel positions reported by each module.,
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d pose, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "