mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[examples] Don't reset encoders when resetting odometry (#6329)
This commit is contained in:
@@ -91,7 +91,6 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
ResetEncoders();
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
units::meter_t{m_rightEncoder.GetDistance()}, pose);
|
||||
|
||||
@@ -30,8 +30,6 @@ void Drivetrain::UpdateOdometry() {
|
||||
}
|
||||
|
||||
void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
m_drivetrainSimulator.SetPose(pose);
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
|
||||
@@ -112,7 +112,6 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
|
||||
}
|
||||
|
||||
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
|
||||
ResetEncoders();
|
||||
m_drivetrainSimulator.SetPose(pose);
|
||||
m_odometry.ResetPosition(m_gyro.GetRotation2d(),
|
||||
units::meter_t{m_leftEncoder.GetDistance()},
|
||||
|
||||
@@ -102,7 +102,6 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(
|
||||
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
|
||||
}
|
||||
|
||||
@@ -123,8 +123,6 @@ public class Drivetrain {
|
||||
|
||||
/** Resets robot odometry. */
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_leftEncoder.reset();
|
||||
m_rightEncoder.reset();
|
||||
m_drivetrainSimulator.setPose(pose);
|
||||
m_odometry.resetPosition(
|
||||
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), pose);
|
||||
|
||||
@@ -179,7 +179,6 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_drivetrainSimulator.setPose(pose);
|
||||
m_odometry.resetPosition(
|
||||
Rotation2d.fromDegrees(getHeading()),
|
||||
|
||||
Reference in New Issue
Block a user