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();