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

[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

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -72,7 +72,9 @@ public class Drivetrain {
m_leftEncoder.reset();
m_rightEncoder.reset();
m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -55,6 +55,8 @@ public class Drivetrain {
private final DifferentialDrivePoseEstimator m_poseEstimator =
new DifferentialDrivePoseEstimator(
m_gyro.getRotation2d(),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
new Pose2d(),
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -99,7 +99,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(pose, m_gyro.getRotation2d(), getCurrentWheelDistances());
m_odometry.resetPosition(m_gyro.getRotation2d(), getCurrentWheelDistances(), pose);
}
/**

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -57,8 +57,8 @@ public class Drivetrain {
private final MecanumDrivePoseEstimator m_poseEstimator =
new MecanumDrivePoseEstimator(
m_gyro.getRotation2d(),
new Pose2d(),
getCurrentDistances(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -64,7 +64,9 @@ public class DriveSubsystem extends SubsystemBase {
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
@Override
@@ -99,7 +101,8 @@ public class DriveSubsystem extends SubsystemBase {
*/
public void resetOdometry(Pose2d pose) {
resetEncoders();
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_odometry.resetPosition(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/**

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
View File

@@ -73,7 +73,9 @@ public class Drivetrain {
m_leftEncoder.reset();
m_rightEncoder.reset();
m_odometry = new DifferentialDriveOdometry(m_gyro.getRotation2d());
m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/**
@@ -116,7 +118,8 @@ public class Drivetrain {
* @param pose The position to reset to.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_odometry.resetPosition(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/**

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -57,7 +57,8 @@ public class Drivetrain {
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackWidth);
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(m_gyro.getRotation2d());
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
// Gains are for example purposes only - must be determined for your own
// robot!
@@ -128,7 +129,8 @@ public class Drivetrain {
m_leftEncoder.reset();
m_rightEncoder.reset();
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_odometry.resetPosition(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
}
/** Check the current robot pose. */

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
View File

@@ -80,7 +80,11 @@ public class DriveSubsystem extends SubsystemBase {
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
m_odometry =
new DifferentialDriveOdometry(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance());
if (RobotBase.isSimulation()) { // If our robot is simulated
// This class simulates our drivetrain's motion around the field.
@@ -174,7 +178,11 @@ public class DriveSubsystem extends SubsystemBase {
public void resetOdometry(Pose2d pose) {
resetEncoders();
m_drivetrainSimulator.setPose(pose);
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
m_odometry.resetPosition(
Rotation2d.fromDegrees(getHeading()),
m_leftEncoder.getDistance(),
m_rightEncoder.getDistance(),
pose);
}
/**

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -37,10 +37,10 @@ public class Drivetrain {
m_kinematics,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
});
public Drivetrain() {

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -62,10 +62,10 @@ public class DriveSubsystem extends SubsystemBase {
DriveConstants.kDriveKinematics,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
/** Creates a new DriveSubsystem. */
@@ -100,14 +100,14 @@ public class DriveSubsystem extends SubsystemBase {
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
pose,
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
},
pose);
}
/**

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -48,13 +48,13 @@ public class Drivetrain {
Nat.N7(),
Nat.N5(),
m_gyro.getRotation2d(),
new Pose2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
},
new Pose2d(),
m_kinematics,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.05, 0.05, 0.05, 0.05),
VecBuilder.fill(Units.degreesToRadians(0.01), 0.01, 0.01, 0.01, 0.01),