diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp index 9f1b12bc9f..3e5326c86c 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp @@ -50,7 +50,8 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, units::second_t period) { auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) + xSpeed, ySpeed, rot, + m_poseEstimator.GetEstimatedPosition().Rotation()) : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, period)); wheelSpeeds.Desaturate(kMaxSpeed); diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp index a365f7f22c..188e944504 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp @@ -15,7 +15,8 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, auto states = m_kinematics.ToSwerveModuleStates(frc::ChassisSpeeds::Discretize( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.GetRotation2d()) + xSpeed, ySpeed, rot, + m_poseEstimator.GetEstimatedPosition().Rotation()) : frc::ChassisSpeeds{xSpeed, ySpeed, rot}, period)); 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 4b643bd259..b5c34c47bf 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 @@ -147,7 +147,7 @@ public class Drivetrain { ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) : new ChassisSpeeds(xSpeed, ySpeed, rot), periodSeconds)); mecanumDriveWheelSpeeds.desaturate(kMaxSpeed); 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 923af5b21e..6eb80e0fe5 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 @@ -71,7 +71,7 @@ public class Drivetrain { ChassisSpeeds.discretize( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, m_gyro.getRotation2d()) + xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation()) : new ChassisSpeeds(xSpeed, ySpeed, rot), periodSeconds)); SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);