diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index bdc624c511..b4ad1db20a 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -75,7 +75,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
- frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+ frc::DifferentialDriveOdometry m_odometry{
+ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
index 9de1289277..e5a87fe9ca 100644
--- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
@@ -79,7 +79,9 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::DifferentialDrivePoseEstimator m_poseEstimator{
- frc::Rotation2d{},
+ m_gyro.GetRotation2d(),
+ units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()},
frc::Pose2d{},
{0.01, 0.01, 0.01, 0.01, 0.01},
{0.1, 0.1, 0.1},
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index 417368415d..9120fa6c77 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -126,6 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() {
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
- m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(),
- getCurrentWheelDistances());
+ m_odometry.ResetPosition(m_gyro.GetRotation2d(), getCurrentWheelDistances(),
+ pose);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
index 43f80933fc..a74aff60d9 100644
--- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
@@ -77,9 +77,9 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{
- 0_deg,
- frc::Pose2d{},
+ m_gyro.GetRotation2d(),
GetCurrentDistances(),
+ frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index 4028cb7a31..f8ec1db47b 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -16,7 +16,7 @@ DriveSubsystem::DriveSubsystem()
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
- m_odometry{m_gyro.GetRotation2d()} {
+ m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
// gearbox is constructed, you might have to invert the left side instead.
@@ -86,5 +86,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
ResetEncoders();
- m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+ m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+ units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
index e5875e90fd..8c1e6323be 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
@@ -28,7 +28,9 @@ void Drivetrain::UpdateOdometry() {
}
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
- m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+ m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+ units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
frc::Pose2d Drivetrain::GetPose() const {
diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
index e66d9e8726..40cc7153fb 100644
--- a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
@@ -76,7 +76,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
- frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+ frc::DifferentialDriveOdometry m_odometry{
+ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
index 4e445e7f82..93d0889df5 100644
--- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
@@ -33,7 +33,9 @@ void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
m_drivetrainSimulator.SetPose(pose);
- m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+ m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+ units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
void Drivetrain::SimulationPeriodic() {
diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
index f163c9b038..80363b52bc 100644
--- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
@@ -92,7 +92,9 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
- frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+ frc::DifferentialDriveOdometry m_odometry{
+ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
index be6bce0385..15d8596435 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
@@ -109,5 +109,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
ResetEncoders();
m_drivetrainSimulator.SetPose(pose);
- m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+ m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+ units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}, pose);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
index 395133c88c..6f7d808392 100644
--- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
+++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
@@ -154,7 +154,9 @@ class DriveSubsystem : public frc2::SubsystemBase {
frc::ADXRS450_Gyro m_gyro;
// Odometry class for tracking robot pose
- frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+ frc::DifferentialDriveOdometry m_odometry{
+ m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+ units::meter_t{m_rightEncoder.GetDistance()}};
// These classes help simulate our drivetrain.
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index 2fa9944c98..4b60372a64 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -103,7 +103,8 @@ frc::Pose2d DriveSubsystem::GetPose() {
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(
- pose, units::degree_t{GetHeading()},
+ GetHeading(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
- m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
+ m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
+ pose);
}
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
index c9015c6905..f3a7c24cf9 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
@@ -51,9 +51,9 @@ class Drivetrain {
// robot!
frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
frc::Rotation2d{},
- frc::Pose2d{},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()},
+ frc::Pose2d{},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index 43b84045aa..8b4734a0f7 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -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());
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
index 34014bc561..ae7a921058 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
@@ -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)),
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index 1b77080572..50a23cd19c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -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);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
index 7040ea1582..a48fb84d52 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -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),
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index b2fe2f70c3..90625ce59a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -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);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
index 19ddefd044..69288a15a9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
@@ -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);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
index 9ab81a3f24..a9f53a9170 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
@@ -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. */
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
index e1b3363531..406178a8ab 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
@@ -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);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index b3facf7442..6a45f6a3e9 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -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() {
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index 93a1cc4ebe..326efee93f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -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);
}
/**
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
index b6b03b9e5a..c4e1a4c15f 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -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),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
index e1ef116fe0..b995011de8 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -30,10 +30,10 @@ import java.util.function.BiConsumer;
*
* {@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
* loops are faster than the default of 20 ms then you should change the {@link
- * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
- * Matrix, double) nominal delta time}.) {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
- * can be called as infrequently as you want; if you never call it then this class will behave
- * exactly like regular encoder odometry.
+ * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, double, double, Pose2d,
+ * Matrix, Matrix, Matrix, double) nominal delta time}.) {@link
+ * DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you want;
+ * if you never call it then this class will behave exactly like regular encoder odometry.
*
*
The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
@@ -70,6 +70,8 @@ public class DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
@@ -83,12 +85,16 @@ public class DifferentialDrivePoseEstimator {
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle,
+ double leftDistanceMeters,
+ double rightDistanceMeters,
Pose2d initialPoseMeters,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
Matrix visionMeasurementStdDevs) {
this(
gyroAngle,
+ leftDistanceMeters,
+ rightDistanceMeters,
initialPoseMeters,
stateStdDevs,
localMeasurementStdDevs,
@@ -100,6 +106,8 @@ public class DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
+ * @param leftDistanceMeters The distance traveled by the left encoder.
+ * @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
@@ -114,6 +122,8 @@ public class DifferentialDrivePoseEstimator {
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle,
+ double leftDistanceMeters,
+ double rightDistanceMeters,
Pose2d initialPoseMeters,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
@@ -155,7 +165,7 @@ public class DifferentialDrivePoseEstimator {
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
- m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
+ m_observer.setXhat(fillStateVector(initialPoseMeters, leftDistanceMeters, rightDistanceMeters));
}
/**
@@ -209,20 +219,24 @@ public class DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
- * 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.
*
- * @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
+ * @param leftPositionMeters The distance traveled by the left encoder.
+ * @param rightPositionMeters 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 leftPositionMeters,
+ double rightPositionMeters,
+ Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
- m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
+ m_observer.setXhat(fillStateVector(poseMeters, leftPositionMeters, rightPositionMeters));
m_prevTimeSeconds = -1;
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
index af1448b9de..07b0abad38 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -32,7 +32,7 @@ import java.util.function.BiConsumer;
*
{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
- * Pose2d, MecanumDriveWheelPositions, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ * MecanumDriveWheelPositions, Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
*
{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
@@ -70,8 +70,8 @@ public class MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
+ * @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
@@ -85,16 +85,16 @@ public class MecanumDrivePoseEstimator {
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
+ Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
Matrix visionMeasurementStdDevs) {
this(
gyroAngle,
- initialPoseMeters,
wheelPositions,
+ initialPoseMeters,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
@@ -106,8 +106,8 @@ public class MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
- * @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
+ * @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
@@ -122,8 +122,8 @@ public class MecanumDrivePoseEstimator {
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
+ Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
@@ -200,12 +200,12 @@ public class MecanumDrivePoseEstimator {
* The gyroscope angle does not need to be reset in 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) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
index 5cddda0715..e3bdc4e7de 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -34,7 +34,7 @@ import java.util.function.BiConsumer;
*
{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Nat, Nat,
- * Nat, Rotation2d, Pose2d, SwerveModulePosition[], SwerveDriveKinematics, Matrix, Matrix, Matrix,
+ * Nat, Rotation2d, SwerveModulePosition[], Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix,
* double)}.
*
*
{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
@@ -96,8 +96,8 @@ public class SwerveDrivePoseEstimator inputs,
Nat outputs,
Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
@@ -107,8 +107,8 @@ public class SwerveDrivePoseEstimator inputs,
Nat outputs,
Rotation2d gyroAngle,
- Pose2d initialPoseMeters,
SwerveModulePosition[] modulePositions,
+ Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix stateStdDevs,
Matrix localMeasurementStdDevs,
@@ -247,12 +247,12 @@ public class SwerveDrivePoseEstimatorThe gyroscope angle does not need to be reset in 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 modulePositions The current distance measurements and rotations of the swerve modules.
+ * @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
- Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+ Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
index 01395735a6..e8f97f53b1 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -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.
*
- * 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.
*
- * @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;
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
index f04da47c34..40a77e23d7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
@@ -70,12 +70,12 @@ public class MecanumDriveOdometry {
*
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);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
index 6b90de224e..3bf92ba08d 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -77,12 +77,12 @@ public class SwerveDriveOdometry {
*
*
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 "
diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
index f9ba581bbd..25f56eaad1 100644
--- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -12,7 +12,8 @@
using namespace frc;
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
+ const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& initialPose,
const wpi::array& stateStdDevs,
const wpi::array& localMeasurementStdDevs,
const wpi::array& visionMeasurmentStdDevs,
@@ -41,7 +42,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
m_gyroOffset = initialPose.Rotation() - gyroAngle;
m_previousAngle = initialPose.Rotation();
- m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+ m_observer.SetXhat(FillStateVector(initialPose, leftDistance, rightDistance));
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
@@ -50,13 +51,15 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
}
-void DifferentialDrivePoseEstimator::ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle) {
+void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance,
+ const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
- m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
+ m_observer.SetXhat(FillStateVector(pose, leftDistance, rightDistance));
m_prevTime = -1_s;
diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
index e6975309ed..fa07073805 100644
--- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
+++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -12,8 +12,8 @@
using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- const MecanumDriveWheelPositions& wheelPositions,
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
MecanumDriveKinematics kinematics,
const wpi::array& stateStdDevs,
const wpi::array& localMeasurementStdDevs,
@@ -67,8 +67,8 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions) {
+ const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index c4a4311b0f..1ff7a8a0d2 100644
--- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -9,8 +9,11 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
- const Rotation2d& gyroAngle, const Pose2d& initialPose)
- : m_pose(initialPose) {
+ const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& initialPose)
+ : m_pose(initialPose),
+ m_prevLeftDistance(leftDistance),
+ m_prevRightDistance(rightDistance) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 329966fb16..394adaf128 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -10,7 +10,7 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose)
+ const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
@@ -22,7 +22,7 @@ MecanumDriveOdometry::MecanumDriveOdometry(
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions wheelPositions) {
+ const MecanumDriveWheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
MecanumDriveWheelPositions wheelDeltas{
diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
index 7dc0007cab..c66f1c67b3 100644
--- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -56,6 +56,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The gyro angle of the robot.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
@@ -79,7 +81,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
+ const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& initialPose,
const wpi::array& stateStdDevs,
const wpi::array& localMeasurementStdDevs,
const wpi::array& visionMeasurementStdDevs,
@@ -103,14 +106,18 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
- * You NEED to reset your encoders to zero when calling this method. The
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero). 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 pose The estimated pose of the robot on the field.
* @param gyroAngle The current gyro angle.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
+ * @param pose The estimated pose of the robot on the field.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& pose);
/**
* Returns the pose of the robot at the current time as estimated by the
diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
index 84ad3b94e7..2bcab513d7 100644
--- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
- * @param initialPose The starting pose estimate.
* @param wheelPositions The distance measured by each wheel.
+ * @param initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
@@ -82,9 +82,9 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* loop.
*/
MecanumDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
+ const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
- MecanumDriveKinematics kinematics,
+ const Pose2d& initialPose, MecanumDriveKinematics kinematics,
const wpi::array& stateStdDevs,
const wpi::array& localMeasurementStdDevs,
const wpi::array& visionMeasurementStdDevs,
@@ -108,15 +108,19 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
- * The gyroscope angle does not need to be reset in the user's robot code.
+ * IF wheelPositions are unspecified,
+ * You NEED to reset your encoders (to zero).
+ *
+ * The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances measured at each wheel.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions& wheelPositions);
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& pose);
/**
* Gets the pose of the robot at the current time as estimated by the Extended
diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
index 327ca0a779..9291374a25 100644
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -61,9 +61,9 @@ class SwerveDrivePoseEstimator {
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
- * @param initialPose The starting pose estimate.
* @param modulePositions The current distance and rotation
* measurements of the swerve modules.
+ * @param initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
@@ -86,9 +86,9 @@ class SwerveDrivePoseEstimator {
* loop.
*/
SwerveDrivePoseEstimator(
- const Rotation2d& gyroAngle, const Pose2d& initialPose,
- const wpi::array modulePositions,
- SwerveDriveKinematics& kinematics,
+ const Rotation2d& gyroAngle,
+ const wpi::array& modulePositions,
+ const Pose2d& initialPose, SwerveDriveKinematics& kinematics,
const wpi::array& stateStdDevs,
const wpi::array& localMeasurementStdDevs,
const wpi::array& visionMeasurementStdDevs,
@@ -138,17 +138,21 @@ class SwerveDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero).
+ *
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
- * @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
+ * @param pose The position on the field that your robot is at.
*/
void ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle,
- wpi::array modulePositions) {
+ const Rotation2d& gyroAngle,
+ const wpi::array& modulePositions,
+ const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
@@ -285,8 +289,8 @@ class SwerveDrivePoseEstimator {
*/
Pose2d Update(
const Rotation2d& gyroAngle,
- const wpi::array moduleStates,
- const wpi::array modulePositions) {
+ const wpi::array& moduleStates,
+ const wpi::array& modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
moduleStates, modulePositions);
}
@@ -306,8 +310,8 @@ class SwerveDrivePoseEstimator {
*/
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
- const wpi::array moduleStates,
- const wpi::array modulePositions) {
+ const wpi::array& moduleStates,
+ const wpi::array& modulePositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index ded26ceeb5..a1a89ca68c 100644
--- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -27,24 +27,35 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
/**
* Constructs a DifferentialDriveOdometry object.
*
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero).
+ *
* @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.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
- * You NEED to reset your encoders (to zero) when calling this method.
+ * IF leftDistance and rightDistance are unspecified,
+ * You NEED to reset your encoders (to zero).
*
* 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 pose The position on the field that your robot is at.
* @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.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index 1d48cdd4a8..5e949caa64 100644
--- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -33,10 +33,10 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
- explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
- const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions wheelPositions,
- const Pose2d& initialPose = Pose2d{});
+ explicit MecanumDriveOdometry(
+ MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -44,12 +44,13 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* 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 pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by each wheel.
+ * @param pose The position on the field that your robot is at.
*/
- void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions wheelPositions) {
+ void ResetPosition(const Rotation2d& gyroAngle,
+ const MecanumDriveWheelPositions& wheelPositions,
+ const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -74,7 +75,7 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
- const MecanumDriveWheelPositions wheelPositions);
+ const MecanumDriveWheelPositions& wheelPositions);
private:
MecanumDriveKinematics m_kinematics;
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 5e4fad204f..59af152e3c 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -40,7 +40,7 @@ class SwerveDriveOdometry {
*/
SwerveDriveOdometry(
SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle,
- const wpi::array modulePositions,
+ const wpi::array& modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
@@ -49,13 +49,14 @@ class SwerveDriveOdometry {
* 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 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 pose The position on the field that your robot is at.
*/
void ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle,
- const wpi::array modulePositions);
+ const Rotation2d& gyroAngle,
+ const wpi::array& modulePositions,
+ const Pose2d& pose);
/**
* Returns the position of the robot on the field.
@@ -80,7 +81,7 @@ class SwerveDriveOdometry {
*/
const Pose2d& Update(
const Rotation2d& gyroAngle,
- const wpi::array modulePositions);
+ const wpi::array& modulePositions);
private:
SwerveDriveKinematics m_kinematics;
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 86af47f323..706c3f1806 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -11,7 +11,7 @@ namespace frc {
template
SwerveDriveOdometry::SwerveDriveOdometry(
SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle,
- const wpi::array modulePositions,
+ const wpi::array& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
@@ -24,8 +24,9 @@ SwerveDriveOdometry::SwerveDriveOdometry(
template
void SwerveDriveOdometry::ResetPosition(
- const Pose2d& pose, const Rotation2d& gyroAngle,
- const wpi::array modulePositions) {
+ const Rotation2d& gyroAngle,
+ const wpi::array& modulePositions,
+ const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -35,7 +36,7 @@ void SwerveDriveOdometry::ResetPosition(
template
const Pose2d& frc::SwerveDriveOdometry::Update(
const Rotation2d& gyroAngle,
- const wpi::array modulePositions) {
+ const wpi::array& modulePositions) {
auto moduleDeltas =
wpi::array(wpi::empty_array);
for (size_t index = 0; index < modulePositions.size(); index++) {
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
index 36fbc258cf..97f4129d51 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -27,6 +27,8 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
new Rotation2d(),
+ 0,
+ 0,
new Pose2d(),
new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
index 1ccd1589e4..e7eb17eb9e 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -32,8 +32,8 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
- new Pose2d(),
wheelPositions,
+ new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
@@ -136,8 +136,8 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
- new Pose2d(),
wheelPositions,
+ new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
index 24ead117a4..27b07c61d0 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -43,8 +43,8 @@ class SwerveDrivePoseEstimatorTest {
Nat.N7(),
Nat.N5(),
new Rotation2d(),
- new Pose2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
+ new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
@@ -158,8 +158,8 @@ class SwerveDrivePoseEstimatorTest {
Nat.N7(),
Nat.N5(),
new Rotation2d(),
- new Pose2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
+ new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
index f85e8fbf2b..6d15f64251 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
@@ -14,11 +14,11 @@ import org.junit.jupiter.api.Test;
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry =
- new DifferentialDriveOdometry(new Rotation2d());
+ new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
@Test
void testOdometryWithEncoderDistances() {
- m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+ m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, new Pose2d());
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
index 5ddfcdf7f3..8d821e651b 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -34,7 +34,7 @@ class MecanumDriveOdometryTest {
void testMultipleConsecutiveUpdates() {
var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
- m_odometry.resetPosition(new Pose2d(), new Rotation2d(), wheelPositions);
+ m_odometry.resetPosition(new Rotation2d(), wheelPositions, new Pose2d());
m_odometry.update(new Rotation2d(), wheelPositions);
var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
@@ -49,7 +49,7 @@ class MecanumDriveOdometryTest {
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
- m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
+ m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
var pose = m_odometry.update(new Rotation2d(), wheelPositions);
@@ -65,7 +65,7 @@ class MecanumDriveOdometryTest {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
- m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
+ m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
@@ -81,7 +81,7 @@ class MecanumDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
- new Pose2d(new Translation2d(), fieldAngle), gyro, new MecanumDriveWheelPositions());
+ gyro, new MecanumDriveWheelPositions(), new Pose2d(new Translation2d(), fieldAngle));
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
index c5ffe9de53..5b07687f08 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
@@ -87,9 +87,9 @@ class SwerveDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
- new Pose2d(new Translation2d(), fieldAngle),
gyro,
- new SwerveModulePosition[] {zero, zero, zero, zero});
+ new SwerveModulePosition[] {zero, zero, zero, zero},
+ new Pose2d(new Translation2d(), fieldAngle));
var delta = new SwerveModulePosition();
m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
index c0022fd59c..550f376daf 100644
--- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -18,6 +18,8 @@
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d{},
+ 0_m,
+ 0_m,
frc::Pose2d{},
{0.02, 0.02, 0.01, 0.02, 0.02},
{0.01, 0.01, 0.001},
diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
index ff35ad68dc..e0bbc94dcc 100644
--- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -19,8 +19,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
- frc::Pose2d{},
wheelPositions,
+ frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -103,8 +103,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
- frc::Pose2d{},
wheelPositions,
+ frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
index c7362ae278..e76afd75fe 100644
--- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -23,8 +23,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
- frc::Pose2d{},
{fl, fr, bl, br},
+ frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -115,8 +115,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
- frc::Pose2d{},
{fl, fr, bl, br},
+ frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index 276f89d8d4..e4809417fa 100644
--- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -13,7 +13,7 @@ static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
- DifferentialDriveOdometry odometry{45_deg};
+ DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
const auto& pose =
odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});
diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index 253007f0fd..bfaf91b1de 100644
--- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -27,7 +27,7 @@ class MecanumDriveOdometryTest : public ::testing::Test {
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
- odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas);
+ odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{});
odometry.Update(0_deg, wheelDeltas);
auto secondPose = odometry.Update(0_deg, wheelDeltas);
@@ -38,7 +38,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
- odometry.ResetPosition(Pose2d{}, 0_rad, zero);
+ odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
@@ -51,7 +51,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
- odometry.ResetPosition(Pose2d{}, 0_rad, zero);
+ odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(0_deg, MecanumDriveWheelPositions{});
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
- odometry.ResetPosition(Pose2d{}, 90_deg, zero);
+ odometry.ResetPosition(90_deg, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 7c8ff0a97c..8c67ab53b3 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -32,7 +32,7 @@ class SwerveDriveOdometryTest : public ::testing::Test {
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
- m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
+ m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
m_odometry.Update(0_deg, {zero, zero, zero, zero});
@@ -50,7 +50,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModulePosition bl{18.85_m, -90_deg};
SwerveModulePosition br{42.15_m, -26.565_deg};
- m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
+ m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
@@ -59,7 +59,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
- m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
+ m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{});
SwerveModulePosition position{0.5_m, 0_deg};