diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp index 756deaae6a..7746099c72 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -103,6 +103,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { {&m_drive}); + // Reset odometry to the starting pose of the trajectory. + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + // no auto return new frc2::SequentialCommandGroup( std::move(mecanumControllerCommand), diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index 33d53c6477..2f39216646 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -84,6 +84,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); }, {&m_drive}); + // Reset odometry to the starting pose of the trajectory. + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + // no auto return new frc2::SequentialCommandGroup( std::move(ramseteCommand), diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp index dd418d2a75..4bb69bb211 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -83,6 +83,9 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { {&m_drive}); + // Reset odometry to the starting pose of the trajectory. + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + // no auto return new frc2::SequentialCommandGroup( std::move(swerveControllerCommand), std::move(swerveControllerCommand), diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java index 1ec1fc9ca8..91055c6fc2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -133,6 +133,9 @@ public class RobotContainer { m_robotDrive ); + // Reset odometry to the starting pose of the trajectory. + m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()); + // Run path following command, then stop at the end. return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index 8bb4e1d1ae..bb8dd28437 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -135,6 +135,9 @@ public class RobotContainer { m_robotDrive ); + // Reset odometry to the starting pose of the trajectory. + m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()); + // Run path following command, then stop at the end. return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index 26e6f22043..544cb1ad5b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -117,6 +117,9 @@ public class RobotContainer { ); + // Reset odometry to the starting pose of the trajectory. + m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()); + // Run path following command, then stop at the end. return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); }