diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index b4b3c49ec4..8591da4cfd 100644 --- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -10,34 +10,12 @@ using namespace frc; DifferentialDriveOdometry::DifferentialDriveOdometry( - DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle, - const Pose2d& initialPose) - : m_kinematics(kinematics), m_pose(initialPose) { + const Rotation2d& gyroAngle, const Pose2d& initialPose) + : m_pose(initialPose) { m_previousAngle = m_pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; } -const Pose2d& DifferentialDriveOdometry::UpdateWithTime( - 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); - - auto newPose = m_pose.Exp( - {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()}); - - m_previousAngle = angle; - m_pose = {newPose.Translation(), angle}; - - return m_pose; -} - const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index f67330aa47..379839df6b 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -23,33 +23,24 @@ namespace frc { * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - * There are two ways of tracking the robot's position on the field with this - * class: one involving using encoder velocities and the other involving encoder - * positions. It is very important that only one type of odometry is used with - * each instantiation of this class. - * - * Note: If you are using the encoder positions / distances method, it is - * important that you reset your encoders to zero before using this class. Any - * subsequent pose resets also require the encoders to be reset to zero. + * It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. */ class DifferentialDriveOdometry { public: /** * 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, + explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle, const Pose2d& initialPose = Pose2d()); /** * Resets the robot's position on the field. * - * If you are using the encoder distances method instead of the velocity - * method, you NEED to reset your encoders (to zero) when calling this method. + * You NEED to reset your encoders (to zero) when calling this method. * * The gyroscope angle does not need to be reset here on the user's robot * code. The library automatically takes care of offsetting the gyro angle. @@ -72,24 +63,6 @@ class DifferentialDriveOdometry { */ const Pose2d& GetPose() const { return m_pose; } - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method takes in the current time as - * a parameter to calculate period (difference between two timestamps). The - * period is used to calculate the change in distance from a velocity. This - * also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. - * - * @param currentTime The current time. - * @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& gyroAngle, - const DifferentialDriveWheelSpeeds& wheelSpeeds); - /** * Updates the robot position on the field using distance measurements from * encoders. This method is more numerically accurate than using velocities to @@ -104,30 +77,9 @@ class DifferentialDriveOdometry { const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance); - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method automatically calculates - * the current time to calculate period (difference between two timestamps). - * The period is used to calculate the change in distance from a velocity. - * This also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. - * - * @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& gyroAngle, - const DifferentialDriveWheelSpeeds& wheelSpeeds) { - return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle, - wheelSpeeds); - } - private: - DifferentialDriveKinematics m_kinematics; Pose2d m_pose; - units::second_t m_previousTime = -1_s; Rotation2d m_gyroOffset; Rotation2d m_previousAngle; diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index 1feae3a1f8..8acaf92314 100644 --- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -15,52 +15,8 @@ static constexpr double kEpsilon = 1E-9; using namespace frc; -TEST(DifferentialDriveOdometry, OneIteration) { - DifferentialDriveKinematics kinematics{0.381_m * 2}; - DifferentialDriveOdometry odometry{kinematics, 0_rad}; - - 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); - - EXPECT_NEAR(pose.Translation().X().to(), 0.02, kEpsilon); - EXPECT_NEAR(pose.Translation().Y().to(), 0.0, kEpsilon); - EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, kEpsilon); -} - -TEST(DifferentialDriveOdometry, QuarterCircle) { - DifferentialDriveKinematics kinematics{0.381_m * 2}; - DifferentialDriveOdometry odometry{kinematics, 0_rad}; - - 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()); - const auto& pose = - odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds); - - EXPECT_NEAR(pose.Translation().X().to(), 5.0, kEpsilon); - 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); -} - TEST(DifferentialDriveOdometry, EncoderDistances) { - DifferentialDriveKinematics kinematics{0.381_m * 2}; - DifferentialDriveOdometry odometry{kinematics, Rotation2d(45_deg)}; + DifferentialDriveOdometry odometry{Rotation2d(45_deg)}; const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m, units::meter_t(5 * wpi::math::pi)); diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp index 8dcc1ea00c..4d9c1ed699 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp @@ -7,11 +7,6 @@ #include "Drivetrain.h" -frc::DifferentialDriveWheelSpeeds Drivetrain::GetSpeeds() const { - return {units::meters_per_second_t(m_leftEncoder.GetRate()), - units::meters_per_second_t(m_rightEncoder.GetRate())}; -} - void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { const auto leftOutput = m_leftPIDController.Calculate( m_leftEncoder.GetRate(), speeds.left.to()); @@ -28,5 +23,6 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed, } void Drivetrain::UpdateOdometry() { - m_odometry.Update(GetAngle(), GetSpeeds()); + m_odometry.Update(GetAngle(), units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); } diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index 38a0376f1c..3032bd9958 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -31,6 +31,9 @@ class Drivetrain { kEncoderResolution); m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / kEncoderResolution); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); } /** @@ -46,7 +49,6 @@ class Drivetrain { static constexpr units::radians_per_second_t kMaxAngularSpeed{ wpi::math::pi}; // 1/2 rotation per second - frc::DifferentialDriveWheelSpeeds GetSpeeds() const; void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); void Drive(units::meters_per_second_t xSpeed, units::radians_per_second_t rot); @@ -74,5 +76,5 @@ class Drivetrain { frc::AnalogGyro m_gyro{0}; frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; - frc::DifferentialDriveOdometry m_odometry{m_kinematics, GetAngle()}; + frc::DifferentialDriveOdometry m_odometry{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 b188556b2d..be5f82a0e2 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -19,17 +19,19 @@ DriveSubsystem::DriveSubsystem() m_right2{kRightMotor2Port}, m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]}, m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]}, - m_odometry{kDriveKinematics, - frc::Rotation2d(units::degree_t(GetHeading()))} { + m_odometry{frc::Rotation2d(units::degree_t(GetHeading()))} { // Set the distance per pulse for the encoders m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + + ResetEncoders(); } void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::degree_t(GetHeading())), - GetWheelSpeeds()); + units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); } void DriveSubsystem::ArcadeDrive(double fwd, double rot) { @@ -74,6 +76,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() { } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + ResetEncoders(); m_odometry.ResetPosition(pose, frc::Rotation2d(units::degree_t(GetHeading()))); } 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 530edbfbc9..2a98f2b51f 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 @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.kinematics; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Twist2d; @@ -21,19 +20,11 @@ import edu.wpi.first.wpilibj.geometry.Twist2d; * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. * - *

There are two ways of tracking the robot's position on the field with this - * class: one involving using encoder velocities and the other involving encoder - * positions. It is very important that only one type of odometry is used with - * each instantiation of this class. - * - *

Note: If you are using the encoder positions / distances method, it is important - * that you reset your encoders to zero before using this class. Any subsequent pose - * resets also require the encoders to be reset to zero. + *

It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. */ public class DifferentialDriveOdometry { - private final DifferentialDriveKinematics m_kinematics; private Pose2d m_poseMeters; - private double m_prevTimeSeconds = -1; private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; @@ -44,13 +35,11 @@ public class DifferentialDriveOdometry { /** * 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, Rotation2d gyroAngle, + public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) { - m_kinematics = kinematics; m_poseMeters = initialPoseMeters; m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = initialPoseMeters.getRotation(); @@ -59,18 +48,16 @@ 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. + * @param gyroAngle The angle reported by the gyroscope. */ - public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle) { - this(kinematics, gyroAngle, new Pose2d()); + public DifferentialDriveOdometry(Rotation2d gyroAngle) { + this(gyroAngle, new Pose2d()); } /** * Resets the robot's position on the field. * - *

If you are using the encoder distances method instead of the velocity method, - * you NEED to reset your encoders (to zero) when calling this method. + *

You NEED to reset your encoders (to zero) when calling this method. * *

The gyroscope angle does not need to be reset here on the user's robot code. * The library automatically takes care of offsetting the gyro angle. @@ -96,37 +83,6 @@ public class DifferentialDriveOdometry { return m_poseMeters; } - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method takes in the current time as - * a parameter to calculate period (difference between two timestamps). The - * period is used to calculate the change in distance from a velocity. This - * also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. - * - * @param currentTimeSeconds The current time in seconds. - * @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 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, - chassisState.vyMetersPerSecond * period, - angle.minus(m_previousAngle).getRadians())); - - m_previousAngle = angle; - - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); - return m_poseMeters; - } /** * Updates the robot position on the field using distance measurements from encoders. This @@ -157,22 +113,4 @@ public class DifferentialDriveOdometry { m_poseMeters = new Pose2d(newPose.getTranslation(), angle); return m_poseMeters; } - - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method automatically calculates the - * current time to calculate period (difference between two timestamps). The - * period is used to calculate the change in distance from a velocity. This - * also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. - * - * @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 gyroAngle, - DifferentialDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(Timer.getFPGATimestamp(), - gyroAngle, wheelSpeeds); - } } 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 789526f68c..aabc455344 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,62 +11,15 @@ 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; 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( new Rotation2d()); - @Test - void testOneIteration() { - 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); - - assertAll( - () -> assertEquals(0.02, pose.getTranslation().getX(), kEpsilon), - () -> assertEquals(0.00, pose.getTranslation().getY(), kEpsilon), - () -> assertEquals(0.00, pose.getRotation().getRadians(), kEpsilon) - ); - } - - @Test - void testQuarterCircle() { - 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); - - assertAll( - () -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon), - () -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon), - () -> 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) - ); - } - @Test void testOdometryWithEncoderDistances() { m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45)); 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 9364105414..cc3d2e1f70 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,8 +51,7 @@ public class Drivetrain { private final DifferentialDriveKinematics m_kinematics = new DifferentialDriveKinematics(kTrackWidth); - private final DifferentialDriveOdometry m_odometry - = new DifferentialDriveOdometry(m_kinematics, getAngle()); + private final DifferentialDriveOdometry m_odometry; /** * Constructs a differential drive object. @@ -66,6 +65,11 @@ public class Drivetrain { // resolution. m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); + + m_leftEncoder.reset(); + m_rightEncoder.reset(); + + m_odometry = new DifferentialDriveOdometry(getAngle()); } /** @@ -78,15 +82,6 @@ public class Drivetrain { return Rotation2d.fromDegrees(-m_gyro.getAngle()); } - /** - * Returns the current wheel speeds. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getCurrentSpeeds() { - return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); - } - /** * Sets the desired wheel speeds. * @@ -117,6 +112,6 @@ public class Drivetrain { * Updates the field-relative position. */ public void updateOdometry() { - m_odometry.update(getAngle(), getCurrentSpeeds()); + m_odometry.update(getAngle(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); } } 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 dde3f281b7..ecb640c19c 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 @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kDriveKinematics; import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kEncoderDistancePerPulse; import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kGyroReversed; import static edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants.kLeftEncoderPorts; @@ -57,8 +56,7 @@ 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, - Rotation2d.fromDegrees(getHeading())); + private final DifferentialDriveOdometry m_odometry; /** * Creates a new DriveSubsystem. @@ -67,12 +65,16 @@ public class DriveSubsystem extends SubsystemBase { // Sets the distance per pulse for the encoders m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + + resetEncoders(); + m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); } @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update(Rotation2d.fromDegrees(getHeading()), getWheelSpeeds()); + m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(), + m_rightEncoder.getDistance()); } /** @@ -99,6 +101,7 @@ public class DriveSubsystem extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { + resetEncoders(); m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); }