From 841ef91c0f1b3ca2b7bf4c4ef8f8c744bc9141af Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Fri, 15 Nov 2019 20:34:10 -0500 Subject: [PATCH] Use gyro angle instead of robot angle for odometry (#2081) The odometry classes previously took in the robot angle as an argument, meaning that users had to take care of offsetting the gyro themselves to accurately report the robot angle. This change will make it so that users will not have to worry about resetting gyros and adding offsets themselves, as this will be handled by the odometry classes. --- .../kinematics/DifferentialDriveOdometry.cpp | 8 +++-- .../cpp/kinematics/MecanumDriveOdometry.cpp | 6 +++- .../kinematics/DifferentialDriveOdometry.h | 21 ++++++++---- .../frc/kinematics/MecanumDriveOdometry.h | 21 ++++++++---- .../frc/kinematics/SwerveDriveOdometry.h | 20 +++++++---- .../frc/kinematics/SwerveDriveOdometry.inc | 8 +++-- .../DifferentialDriveOdometryTest.cpp | 21 +++++++++--- .../kinematics/MecanumDriveOdometryTest.cpp | 21 +++++++++--- .../kinematics/SwerveDriveOdometryTest.cpp | 22 +++++++++++-- .../DifferentialDriveBot/include/Drivetrain.h | 2 +- .../examples/MecanumBot/include/Drivetrain.h | 2 +- .../cpp/subsystems/DriveSubsystem.cpp | 6 ++-- .../examples/SwerveBot/include/Drivetrain.h | 2 +- .../kinematics/DifferentialDriveOdometry.java | 30 ++++++++++++----- .../kinematics/MecanumDriveOdometry.java | 31 ++++++++++++----- .../kinematics/SwerveDriveOdometry.java | 33 +++++++++++++------ .../DifferentialDriveOdometryTest.java | 25 ++++++++++++-- .../kinematics/MecanumDriveOdometryTest.java | 22 ++++++++++++- .../kinematics/SwerveDriveOdometryTest.java | 22 ++++++++++++- .../differentialdrivebot/Drivetrain.java | 2 +- .../examples/mecanumbot/Drivetrain.java | 3 +- .../subsystems/DriveSubsystem.java | 5 +-- .../examples/swervebot/Drivetrain.java | 2 +- 23 files changed, 258 insertions(+), 77 deletions(-) diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 418dd0f5ad..49a138e818 100644 --- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -10,18 +10,22 @@ using namespace frc; DifferentialDriveOdometry::DifferentialDriveOdometry( - DifferentialDriveKinematics kinematics, const Pose2d& initialPose) + DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle, + const Pose2d& initialPose) : m_kinematics(kinematics), m_pose(initialPose) { m_previousAngle = m_pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } const Pose2d& DifferentialDriveOdometry::UpdateWithTime( - units::second_t currentTime, const Rotation2d& angle, + units::second_t currentTime, const Rotation2d& gyroAngle, const DifferentialDriveWheelSpeeds& wheelSpeeds) { units::second_t deltaTime = (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; m_previousTime = currentTime; + auto angle = gyroAngle + m_gyroOffset; + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds); static_cast(dtheta); diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index 18e563527d..615635aea7 100644 --- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp +++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -10,18 +10,22 @@ using namespace frc; MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics, + const Rotation2d& gyroAngle, const Pose2d& initialPose) : m_kinematics(kinematics), m_pose(initialPose) { m_previousAngle = m_pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } const Pose2d& MecanumDriveOdometry::UpdateWithTime( - units::second_t currentTime, const Rotation2d& angle, + units::second_t currentTime, const Rotation2d& gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { units::second_t deltaTime = (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; m_previousTime = currentTime; + auto angle = gyroAngle + m_gyroOffset; + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds); static_cast(dtheta); diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index fd41d15ead..eee829f45f 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -33,19 +33,26 @@ class DifferentialDriveOdometry { * Constructs a DifferentialDriveOdometry object. * * @param kinematics The differential drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPose The starting position of the robot on the field. */ explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, + const Rotation2d& gyroAngle, const Pose2d& initialPose = Pose2d()); /** * Resets the robot's position on the field. * + * 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. */ - void ResetPosition(const Pose2d& pose) { + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { m_pose = pose; m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } /** @@ -63,13 +70,13 @@ class DifferentialDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTime The current time. - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * * @return The new pose of the robot. */ const Pose2d& UpdateWithTime(units::second_t currentTime, - const Rotation2d& angle, + const Rotation2d& gyroAngle, const DifferentialDriveWheelSpeeds& wheelSpeeds); /** @@ -80,14 +87,15 @@ class DifferentialDriveOdometry { * This also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * * @return The new pose of the robot. */ - const Pose2d& Update(const Rotation2d& angle, + const Pose2d& Update(const Rotation2d& gyroAngle, const DifferentialDriveWheelSpeeds& wheelSpeeds) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds); + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, + wheelSpeeds); } private: @@ -95,6 +103,7 @@ class DifferentialDriveOdometry { Pose2d m_pose; units::second_t m_previousTime = -1_s; + Rotation2d m_gyroOffset; Rotation2d m_previousAngle; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index c8db78a1fd..697a6b0a26 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -31,19 +31,26 @@ class MecanumDriveOdometry { * Constructs a MecanumDriveOdometry object. * * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPose The starting position of the robot on the field. */ explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics, + const Rotation2d& gyroAngle, const Pose2d& initialPose = Pose2d()); /** * Resets the robot's position on the field. * + * 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. */ - void ResetPosition(const Pose2d& pose) { + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { m_pose = pose; m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } /** @@ -61,13 +68,13 @@ class MecanumDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTime The current time. - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * * @return The new pose of the robot. */ const Pose2d& UpdateWithTime(units::second_t currentTime, - const Rotation2d& angle, + const Rotation2d& gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds); /** @@ -78,14 +85,15 @@ class MecanumDriveOdometry { * This also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * * @return The new pose of the robot. */ - const Pose2d& Update(const Rotation2d& angle, + const Pose2d& Update(const Rotation2d& gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds); + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, + wheelSpeeds); } private: @@ -94,6 +102,7 @@ class MecanumDriveOdometry { units::second_t m_previousTime = -1_s; Rotation2d m_previousAngle; + Rotation2d m_gyroOffset; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index 1dbe1783fa..8093ca5360 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -36,19 +36,26 @@ class SwerveDriveOdometry { * Constructs a SwerveDriveOdometry object. * * @param kinematics The swerve drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPose The starting position of the robot on the field. */ SwerveDriveOdometry(SwerveDriveKinematics kinematics, + const Rotation2d& gyroAngle, const Pose2d& initialPose = Pose2d()); /** * Resets the robot's position on the field. * + * 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. */ - void ResetPosition(const Pose2d& pose) { + void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) { m_pose = pose; m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } /** @@ -66,7 +73,7 @@ class SwerveDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTime The current time. - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param moduleStates The current state of all swerve modules. Please provide * the states in the same order in which you instantiated * your SwerveDriveKinematics. @@ -75,7 +82,7 @@ class SwerveDriveOdometry { */ template const Pose2d& UpdateWithTime(units::second_t currentTime, - const Rotation2d& angle, + const Rotation2d& gyroAngle, ModuleStates&&... moduleStates); /** @@ -86,7 +93,7 @@ class SwerveDriveOdometry { * This also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param moduleStates The current state of all swerve modules. Please provide * the states in the same order in which you instantiated * your SwerveDriveKinematics. @@ -94,9 +101,9 @@ class SwerveDriveOdometry { * @return The new pose of the robot. */ template - const Pose2d& Update(const Rotation2d& angle, + const Pose2d& Update(const Rotation2d& gyroAngle, ModuleStates&&... moduleStates) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, moduleStates...); } @@ -106,6 +113,7 @@ class SwerveDriveOdometry { units::second_t m_previousTime = -1_s; Rotation2d m_previousAngle; + Rotation2d m_gyroOffset; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index 1ff75ac00b..23852d0ecb 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -10,20 +10,24 @@ namespace frc { template SwerveDriveOdometry::SwerveDriveOdometry( - SwerveDriveKinematics kinematics, const Pose2d& initialPose) + SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, + const Pose2d& initialPose) : m_kinematics(kinematics), m_pose(initialPose) { m_previousAngle = m_pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; } template template const Pose2d& frc::SwerveDriveOdometry::UpdateWithTime( - units::second_t currentTime, const Rotation2d& angle, + units::second_t currentTime, const Rotation2d& gyroAngle, ModuleStates&&... moduleStates) { units::second_t deltaTime = (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s; m_previousTime = currentTime; + auto angle = gyroAngle + m_gyroOffset; + auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...); static_cast(dtheta); diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index 2647252a2d..105ed29e97 100644 --- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -17,9 +17,9 @@ using namespace frc; TEST(DifferentialDriveOdometry, OneIteration) { DifferentialDriveKinematics kinematics{0.381_m * 2}; - DifferentialDriveOdometry odometry{kinematics}; + DifferentialDriveOdometry odometry{kinematics, 0_rad}; - odometry.ResetPosition(Pose2d()); + odometry.ResetPosition(Pose2d(), 0_rad); DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps}; odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds()); const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds); @@ -31,9 +31,9 @@ TEST(DifferentialDriveOdometry, OneIteration) { TEST(DifferentialDriveOdometry, QuarterCircle) { DifferentialDriveKinematics kinematics{0.381_m * 2}; - DifferentialDriveOdometry odometry{kinematics}; + DifferentialDriveOdometry odometry{kinematics, 0_rad}; - odometry.ResetPosition(Pose2d()); + odometry.ResetPosition(Pose2d(), 0_rad); DifferentialDriveWheelSpeeds wheelSpeeds{ 0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)}; odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds()); @@ -44,3 +44,16 @@ TEST(DifferentialDriveOdometry, QuarterCircle) { EXPECT_NEAR(pose.Translation().Y().to(), 5.0, kEpsilon); EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, kEpsilon); } + +TEST(DifferentialDriveWheelSpeeds, GyroAngleReset) { + DifferentialDriveKinematics kinematics{0.381_m * 2}; + DifferentialDriveOdometry odometry{kinematics, Rotation2d(90_deg)}; + + odometry.UpdateWithTime(0_s, Rotation2d(90_deg), {}); + const auto& pose = + odometry.UpdateWithTime(1_s, Rotation2d(90_deg), {1_mps, 1_mps}); + + EXPECT_NEAR(pose.Translation().X().to(), 1.0, kEpsilon); + EXPECT_NEAR(pose.Translation().Y().to(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().Degrees().to(), 0.0, kEpsilon); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index 0a6894dcae..5d990bfad3 100644 --- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -18,11 +18,11 @@ class MecanumDriveOdometryTest : public ::testing::Test { Translation2d m_br{-12_m, -12_m}; MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; - MecanumDriveOdometry odometry{kinematics}; + MecanumDriveOdometry odometry{kinematics, 0_rad}; }; TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { - odometry.ResetPosition(Pose2d()); + odometry.ResetPosition(Pose2d(), 0_rad); MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; @@ -35,7 +35,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) { } TEST_F(MecanumDriveOdometryTest, TwoIterations) { - odometry.ResetPosition(Pose2d()); + odometry.ResetPosition(Pose2d(), 0_rad); MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); @@ -47,7 +47,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) { } TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) { - odometry.ResetPosition(Pose2d()); + odometry.ResetPosition(Pose2d(), 0_rad); MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps, 39.986_mps}; odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); @@ -57,3 +57,16 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) { EXPECT_NEAR(pose.Translation().Y().to(), 12, 0.01); EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, 0.01); } + +TEST_F(MecanumDriveOdometryTest, GyroAngleReset) { + odometry.ResetPosition(Pose2d(), Rotation2d(90_deg)); + + MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps}; + + odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{}); + auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds); + + EXPECT_NEAR(pose.Translation().X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.Translation().Y().to(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); +} diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index ab81017ed7..d0399dcdd9 100644 --- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -21,13 +21,13 @@ class SwerveDriveOdometryTest : public ::testing::Test { Translation2d m_br{-12_m, -12_m}; SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; - SwerveDriveOdometry<4> m_odometry{m_kinematics}; + SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad}; }; TEST_F(SwerveDriveOdometryTest, TwoIterations) { SwerveModuleState state{5_mps, Rotation2d()}; - m_odometry.ResetPosition(Pose2d()); + m_odometry.ResetPosition(Pose2d(), 0_rad); m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState()); @@ -47,7 +47,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { SwerveModuleState zero{0_mps, Rotation2d()}; - m_odometry.ResetPosition(Pose2d()); + m_odometry.ResetPosition(Pose2d(), 0_rad); m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero); auto pose = m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br); @@ -56,3 +56,19 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) { EXPECT_NEAR(12.0, pose.Translation().Y().to(), kEpsilon); EXPECT_NEAR(90.0, pose.Rotation().Degrees().to(), kEpsilon); } + +TEST_F(SwerveDriveOdometryTest, GyroAngleReset) { + m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg)); + + SwerveModuleState state{5_mps, Rotation2d()}; + + m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(), + SwerveModuleState(), SwerveModuleState(), + SwerveModuleState()); + auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state, + state, state); + + EXPECT_NEAR(0.5, pose.Translation().X().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Translation().Y().to(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().Degrees().to(), kEpsilon); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index 151404a993..0894ec10e4 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -74,5 +74,5 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; - frc::DifferentialDriveOdometry m_odometry{m_kinematics}; + frc::DifferentialDriveOdometry m_odometry{m_kinematics, GetAngle()}; }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index e9f5714216..5c1ef6c26e 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -71,5 +71,5 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::MecanumDriveOdometry m_odometry{m_kinematics}; + frc::MecanumDriveOdometry m_odometry{m_kinematics, GetAngle()}; }; 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 bc13cec27f..faa9ec2b4a 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -19,7 +19,8 @@ DriveSubsystem::DriveSubsystem() m_right2{kRightMotor2Port}, m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]}, - m_odometry{kDriveKinematics, frc::Pose2d()} { + m_odometry{kDriveKinematics, + frc::Rotation2d(units::degree_t(GetHeading()))} { // Set the distance per pulse for the encoders m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); @@ -70,5 +71,6 @@ double DriveSubsystem::GetTurnRate() { frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { - m_odometry.ResetPosition(pose); + m_odometry.ResetPosition(pose, + frc::Rotation2d(units::degree_t(GetHeading()))); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 745581ac8b..2de6e956d7 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -57,5 +57,5 @@ class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation}; - frc::SwerveDriveOdometry<4> m_odometry{m_kinematics}; + frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, GetAngle()}; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java index 0f411ca95d..68e43d480a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java @@ -30,18 +30,21 @@ public class DifferentialDriveOdometry { private Pose2d m_poseMeters; private double m_prevTimeSeconds = -1; + private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; /** * Constructs a DifferentialDriveOdometry object. * * @param kinematics The differential drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPoseMeters The starting position of the robot on the field. */ - public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, + public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); } @@ -49,24 +52,31 @@ public class DifferentialDriveOdometry { * Constructs a DifferentialDriveOdometry object with the default pose at the origin. * * @param kinematics The differential drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. */ - public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) { - this(kinematics, new Pose2d()); + public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle) { + this(kinematics, gyroAngle, new Pose2d()); } /** * Resets the robot's position on the field. Do NOT zero your encoders if you * call this function at any other time except initialization. * + *

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. */ - public void resetPosition(Pose2d poseMeters) { + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { m_poseMeters = poseMeters; m_previousAngle = poseMeters.getRotation(); + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); } /** * Returns the position of the robot on the field. + * * @return The pose of the robot (x and y are in meters). */ public Pose2d getPoseMeters() { @@ -82,15 +92,17 @@ public class DifferentialDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTimeSeconds The current time in seconds. - * @param angle The current robot angle. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * @return The new pose of the robot. */ - public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelSpeeds) { double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; m_prevTimeSeconds = currentTimeSeconds; + var angle = gyroAngle.plus(m_gyroOffset); + var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds); var newPose = m_poseMeters.exp( new Twist2d(chassisState.vxMetersPerSecond * period, @@ -111,13 +123,13 @@ public class DifferentialDriveOdometry { * also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * @return The new pose of the robot. */ - public Pose2d update(Rotation2d angle, + public Pose2d update(Rotation2d gyroAngle, DifferentialDriveWheelSpeeds wheelSpeeds) { return updateWithTime(Timer.getFPGATimestamp(), - angle, wheelSpeeds); + gyroAngle, wheelSpeeds); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java index 83fae90fe9..22ef83a317 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java @@ -26,17 +26,21 @@ public class MecanumDriveOdometry { private Pose2d m_poseMeters; private double m_prevTimeSeconds = -1; + private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; /** * Constructs a MecanumDriveOdometry object. * * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPoseMeters The starting position of the robot on the field. */ - public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Pose2d initialPoseMeters) { + public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle, + Pose2d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); } @@ -44,23 +48,30 @@ public class MecanumDriveOdometry { * Constructs a MecanumDriveOdometry object with the default pose at the origin. * * @param kinematics The mecanum drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. */ - public MecanumDriveOdometry(MecanumDriveKinematics kinematics) { - this(kinematics, new Pose2d()); + public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) { + this(kinematics, gyroAngle, new Pose2d()); } /** * Resets the robot's position on the field. * + *

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. */ - public void resetPosition(Pose2d poseMeters) { + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { m_poseMeters = poseMeters; m_previousAngle = poseMeters.getRotation(); + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); } /** * Returns the position of the robot on the field. + * * @return The pose of the robot (x and y are in meters). */ public Pose2d getPoseMeters() { @@ -76,15 +87,17 @@ public class MecanumDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTimeSeconds The current time in seconds. - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * @return The new pose of the robot. */ - public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; m_prevTimeSeconds = currentTimeSeconds; + var angle = gyroAngle.plus(m_gyroOffset); + var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds); var newPose = m_poseMeters.exp( new Twist2d(chassisState.vxMetersPerSecond * period, @@ -104,13 +117,13 @@ public class MecanumDriveOdometry { * also takes in an angle parameter which is used instead of the * angular rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param wheelSpeeds The current wheel speeds. * @return The new pose of the robot. */ - public Pose2d update(Rotation2d angle, + public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(Timer.getFPGATimestamp(), angle, + return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, wheelSpeeds); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java index 2cfea4eaca..c08070c916 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java @@ -26,17 +26,21 @@ public class SwerveDriveOdometry { private Pose2d m_poseMeters; private double m_prevTimeSeconds = -1; + private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; /** * Constructs a SwerveDriveOdometry object. * * @param kinematics The swerve drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. * @param initialPose The starting position of the robot on the field. */ - public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) { + public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle, + Pose2d initialPose) { m_kinematics = kinematics; m_poseMeters = initialPose; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPose.getRotation(); } @@ -44,23 +48,30 @@ public class SwerveDriveOdometry { * Constructs a SwerveDriveOdometry object with the default pose at the origin. * * @param kinematics The swerve drive kinematics for your drivetrain. + * @param gyroAngle The angle reported by the gyroscope. */ - public SwerveDriveOdometry(SwerveDriveKinematics kinematics) { - this(kinematics, new Pose2d()); + public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) { + this(kinematics, gyroAngle, new Pose2d()); } /** * Resets the robot's position on the field. * - * @param pose The position on the field that your robot is at. + *

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. */ - public void resetPosition(Pose2d pose) { + public void resetPosition(Pose2d pose, Rotation2d gyroAngle) { m_poseMeters = pose; m_previousAngle = pose.getRotation(); + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); } /** * Returns the position of the robot on the field. + * * @return The pose of the robot (x and y are in meters). */ public Pose2d getPoseMeters() { @@ -76,17 +87,19 @@ public class SwerveDriveOdometry { * angular rate that is calculated from forward kinematics. * * @param currentTimeSeconds The current time in seconds. - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param moduleStates The current state of all swerve modules. Please provide * the states in the same order in which you instantiated your * SwerveDriveKinematics. * @return The new pose of the robot. */ - public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle, + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) { double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; m_prevTimeSeconds = currentTimeSeconds; + var angle = gyroAngle.plus(m_gyroOffset); + var chassisState = m_kinematics.toChassisSpeeds(moduleStates); var newPose = m_poseMeters.exp( new Twist2d(chassisState.vxMetersPerSecond * period, @@ -107,13 +120,13 @@ public class SwerveDriveOdometry { * also takes in an angle parameter which is used instead of the angular * rate that is calculated from forward kinematics. * - * @param angle The angle of the robot. + * @param gyroAngle The angle reported by the gyroscope. * @param moduleStates The current state of all swerve modules. Please provide * the states in the same order in which you instantiated your * SwerveDriveKinematics. * @return The new pose of the robot. */ - public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) { - return updateWithTime(Timer.getFPGATimestamp(), angle, moduleStates); + public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) { + return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java index 4562ed2603..3870bcbc74 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -19,11 +20,12 @@ class DifferentialDriveOdometryTest { private static final double kEpsilon = 1E-9; private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(0.381 * 2); - private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics); + private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics, + new Rotation2d()); @Test void testOneIteration() { - m_odometry.resetPosition(new Pose2d()); + m_odometry.resetPosition(new Pose2d(), new Rotation2d()); var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02); m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds()); var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds); @@ -37,7 +39,7 @@ class DifferentialDriveOdometryTest { @Test void testQuarterCircle() { - m_odometry.resetPosition(new Pose2d()); + m_odometry.resetPosition(new Pose2d(), new Rotation2d()); var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI); m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds()); var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds); @@ -48,4 +50,21 @@ class DifferentialDriveOdometryTest { () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon) ); } + + @Test + void testGyroAngleReset() { + var gyro = Rotation2d.fromDegrees(90.0); + var fieldAngle = Rotation2d.fromDegrees(0.0); + m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro); + var speeds = new DifferentialDriveWheelSpeeds(1.0, 1.0); + m_odometry.updateWithTime(0.0, gyro, new DifferentialDriveWheelSpeeds()); + var pose = m_odometry.updateWithTime(1.0, gyro, speeds); + + assertAll( + () -> assertEquals(1.0, pose.getTranslation().getX(), kEpsilon), + () -> assertEquals(0.0, pose.getTranslation().getY(), kEpsilon), + () -> assertEquals(0.0, pose.getRotation().getRadians(), kEpsilon) + ); + } + } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java index 04accebe58..4619bf0dd0 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java @@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics; import org.junit.jupiter.api.Test; +import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; @@ -21,10 +22,12 @@ class MecanumDriveOdometryTest { private final Translation2d m_bl = new Translation2d(-12, 12); private final Translation2d m_br = new Translation2d(-12, -12); + private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br); - private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics); + private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics, + new Rotation2d()); @Test void testMultipleConsecutiveUpdates() { @@ -71,4 +74,21 @@ class MecanumDriveOdometryTest { ); } + @Test + void testGyroAngleReset() { + var gyro = Rotation2d.fromDegrees(90.0); + var fieldAngle = Rotation2d.fromDegrees(0.0); + m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro); + var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, + 3.536, 3.536); + m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds()); + var pose = m_odometry.updateWithTime(1.0, gyro, speeds); + + assertAll( + () -> assertEquals(5.0, pose.getTranslation().getX(), 0.1), + () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1), + () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1) + ); + } + } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java index c945b8bd4e..85b832c14b 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java @@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics; import org.junit.jupiter.api.Test; +import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; @@ -24,7 +25,8 @@ class SwerveDriveOdometryTest { private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br); - private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics); + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, + new Rotation2d()); @Test void testTwoIterations() { @@ -73,4 +75,22 @@ class SwerveDriveOdometryTest { () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01) ); } + + @Test + void testGyroAngleReset() { + var gyro = Rotation2d.fromDegrees(90.0); + var fieldAngle = Rotation2d.fromDegrees(0.0); + m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro); + var state = new SwerveModuleState(); + m_odometry.updateWithTime(0.0, gyro, state, state, state, state); + state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0)); + var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state); + + assertAll( + () -> assertEquals(1.0, pose.getTranslation().getX(), 0.1), + () -> assertEquals(0.00, pose.getTranslation().getY(), 0.1), + () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1) + ); + } + } 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 dd5b14cae4..09df9e3c48 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 @@ -51,7 +51,7 @@ public class Drivetrain { = new DifferentialDriveKinematics(kTrackWidth); private final DifferentialDriveOdometry m_odometry - = new DifferentialDriveOdometry(m_kinematics); + = new DifferentialDriveOdometry(m_kinematics, getAngle()); /** * Constructs a differential drive object. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index 2d636aa819..73dfb67f3f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -52,7 +52,8 @@ public class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation ); - private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics); + private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics, + getAngle()); /** * Constructs a MecanumDrive and resets the gyro. 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 4b84f91243..a9f272bd90 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 @@ -57,7 +57,8 @@ public class DriveSubsystem extends SubsystemBase { private final Gyro m_gyro = new ADXRS450_Gyro(); // Odometry class for tracking robot pose - DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics); + DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(kDriveKinematics, + Rotation2d.fromDegrees(getHeading())); /** * Creates a new DriveSubsystem. @@ -93,7 +94,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_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); } /** 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 0ec09f085b..828a94b927 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,7 +37,7 @@ public class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation ); - private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics); + private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics, getAngle()); public Drivetrain() { m_gyro.reset();