diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp index 3e423892d0..856ec99911 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp @@ -66,10 +66,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // Pass the config config); - // Reset odometry to the starting pose of the trajectory. - m_drive.ResetOdometry(exampleTrajectory.InitialPose()); - - frc2::CommandPtr mecanumDriveCommand = + frc2::CommandPtr mecanumControllerCommand = frc2::MecanumControllerCommand( exampleTrajectory, [this]() { return m_drive.GetPose(); }, @@ -110,8 +107,16 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { {&m_drive}) .ToPtr(); + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. return frc2::cmd::Sequence( - std::move(mecanumDriveCommand), - frc2::InstantCommand([this] { m_drive.Drive(0, 0, 0, false); }, {}) + frc2::InstantCommand( + [this, &exampleTrajectory]() { + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + }, + {}) + .ToPtr(), + std::move(mecanumControllerCommand), + frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {}) .ToPtr()); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index e880af2d5e..5fa8c62011 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -81,10 +81,14 @@ frc2::CommandPtr 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()); - - return std::move(ramseteCommand) - .BeforeStarting( + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. + return frc2::cmd::RunOnce( + [this, &exampleTrajectory] { + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + }, + {}) + .AndThen(std::move(ramseteCommand)) + .AndThen( frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {})); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp index 9865e00eb5..2ca4b71893 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -74,10 +74,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi}, units::radian_t{std::numbers::pi}); - // Reset odometry to the starting pose of the trajectory. - m_drive.ResetOdometry(exampleTrajectory.InitialPose()); - - frc2::CommandPtr swerveDriveCommand = + frc2::CommandPtr swerveControllerCommand = frc2::SwerveControllerCommand<4>( exampleTrajectory, [this]() { return m_drive.GetPose(); }, @@ -92,8 +89,16 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { {&m_drive}) .ToPtr(); + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. return frc2::cmd::Sequence( - std::move(swerveDriveCommand), + frc2::InstantCommand( + [this, &exampleTrajectory]() { + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + }, + {}) + .ToPtr(), + std::move(swerveControllerCommand), frc2::InstantCommand( [this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {}) .ToPtr()); 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 74c088fba4..d278f054c4 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 @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveCo import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.MecanumControllerCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -85,7 +86,7 @@ public class RobotContainer { // Add kinematics to ensure max speed is actually obeyed .setKinematics(DriveConstants.kDriveKinematics); - // An example trajectory to follow. All units in meters. + // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction @@ -121,10 +122,11 @@ public class RobotContainer { m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages 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)); + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. + return Commands.sequence( + new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())), + mecanumControllerCommand, + new InstantCommand(() -> 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 9f0980a0e9..4bbf45c29b 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 @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj.examples.ramsetecommand; -import static edu.wpi.first.wpilibj.XboxController.Button; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -17,11 +15,13 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -97,7 +97,7 @@ public class RobotContainer { // Apply the voltage constraint .addConstraint(autoVoltageConstraint); - // An example trajectory to follow. All units in meters. + // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction @@ -126,10 +126,10 @@ public class RobotContainer { m_robotDrive::tankDriveVolts, 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)); + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. + return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())) + .andThen(ramseteCommand) + .andThen(Commands.runOnce(() -> 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 cf0ff8c5ef..1f8e3e57ea 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 @@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleCo import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -82,7 +84,7 @@ public class RobotContainer { // Add kinematics to ensure max speed is actually obeyed .setKinematics(DriveConstants.kDriveKinematics); - // An example trajectory to follow. All units in meters. + // An example trajectory to follow. All units in meters. Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction @@ -111,10 +113,11 @@ public class RobotContainer { m_robotDrive::setModuleStates, 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 swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false)); + // Reset odometry to the initial pose of the trajectory, run path following + // command, then stop at the end. + return Commands.sequence( + new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())), + swerveControllerCommand, + new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false))); } }