diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp index d3a106b3935..13eb9f9ac46 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp @@ -39,7 +39,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -51,9 +51,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp index 90aa789ab4c..1bf4dcf6df8 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp @@ -60,6 +60,6 @@ void RobotContainer::DisablePIDSubsystems() { m_arm.Disable(); } -frc2::Command* RobotContainer::GetAutonomousCommand() { - return nullptr; +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { + return frc2::cmd::None(); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h index 67d194c6941..ab77985e541 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include "Constants.h" @@ -28,7 +29,7 @@ class RobotContainer { */ void DisablePIDSubsystems(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp index c7eab485a5d..c4c1661d699 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp index dce4b95c18f..a8704165a5b 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp @@ -4,6 +4,7 @@ #include "RobotContainer.h" +#include #include RobotContainer::RobotContainer() { @@ -34,6 +35,6 @@ void RobotContainer::ConfigureButtonBindings() { .OnFalse(m_drive.SetMaxOutputCommand(1.0)); } -frc2::Command* RobotContainer::GetAutonomousCommand() { - return nullptr; +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { + return frc2::cmd::None(); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h index 08a4dbea307..7668c03e9a9 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include "Constants.h" @@ -24,7 +25,7 @@ class RobotContainer { public: RobotContainer(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp index c7eab485a5d..c4c1661d699 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp index bcb7e7367b6..37ae1aac11a 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -4,6 +4,8 @@ #include "RobotContainer.h" +#include + #include "commands/DriveDistanceProfiled.h" RobotContainer::RobotContainer() { @@ -60,7 +62,7 @@ void RobotContainer::ConfigureButtonBindings() { .WithTimeout(10_s)); } -frc2::Command* RobotContainer::GetAutonomousCommand() { +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // Runs the chosen command in autonomous - return nullptr; + return frc2::cmd::None(); } diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h index 4700a70564c..6bfda5828b4 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h @@ -23,7 +23,7 @@ class RobotContainer { public: RobotContainer(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp index c7eab485a5d..c4c1661d699 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp index 50b9900c225..7b076840b9b 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp @@ -64,7 +64,7 @@ void RobotContainer::ConfigureButtonBindings() { .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {})); } -frc2::Command* RobotContainer::GetAutonomousCommand() { +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // no auto - return nullptr; + return frc2::cmd::None(); } diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h index 041812e8e45..0ecb7286d9f 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include "Constants.h" @@ -28,7 +29,7 @@ class RobotContainer { public: RobotContainer(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller @@ -39,8 +40,5 @@ class RobotContainer { // The robot's subsystems DriveSubsystem m_drive; - // The chooser for the autonomous routines - frc::SendableChooser m_chooser; - void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp index c7eab485a5d..c4c1661d699 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp index 0f691e00389..3e423892d0c 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -47,7 +48,7 @@ void RobotContainer::ConfigureButtonBindings() { .OnFalse(&m_driveFullSpeed); } -frc2::Command* RobotContainer::GetAutonomousCommand() { +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // Set up config for trajectory frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, AutoConstants::kMaxAcceleration); @@ -65,48 +66,52 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { // Pass the config config); - frc2::MecanumControllerCommand mecanumControllerCommand( - exampleTrajectory, [this]() { return m_drive.GetPose(); }, - - frc::SimpleMotorFeedforward(ks, kv, ka), - DriveConstants::kDriveKinematics, - - frc::PIDController{AutoConstants::kPXController, 0, 0}, - frc::PIDController{AutoConstants::kPYController, 0, 0}, - frc::ProfiledPIDController( - AutoConstants::kPThetaController, 0, 0, - AutoConstants::kThetaControllerConstraints), - - AutoConstants::kMaxSpeed, - - [this]() { - return frc::MecanumDriveWheelSpeeds{ - units::meters_per_second_t{m_drive.GetFrontLeftEncoder().GetRate()}, - units::meters_per_second_t{ - m_drive.GetFrontRightEncoder().GetRate()}, - units::meters_per_second_t{m_drive.GetRearLeftEncoder().GetRate()}, - units::meters_per_second_t{ - m_drive.GetRearRightEncoder().GetRate()}}; - }, - - frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0}, - frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0}, - frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0}, - frc::PIDController{DriveConstants::kPRearRightVel, 0, 0}, - - [this](units::volt_t frontLeft, units::volt_t rearLeft, - units::volt_t frontRight, units::volt_t rearRight) { - m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight, - rearRight); - }, - - {&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), - frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})); + frc2::CommandPtr mecanumDriveCommand = + frc2::MecanumControllerCommand( + exampleTrajectory, [this]() { return m_drive.GetPose(); }, + + frc::SimpleMotorFeedforward(ks, kv, ka), + DriveConstants::kDriveKinematics, + + frc::PIDController{AutoConstants::kPXController, 0, 0}, + frc::PIDController{AutoConstants::kPYController, 0, 0}, + frc::ProfiledPIDController( + AutoConstants::kPThetaController, 0, 0, + AutoConstants::kThetaControllerConstraints), + + AutoConstants::kMaxSpeed, + + [this]() { + return frc::MecanumDriveWheelSpeeds{ + units::meters_per_second_t{ + m_drive.GetFrontLeftEncoder().GetRate()}, + units::meters_per_second_t{ + m_drive.GetFrontRightEncoder().GetRate()}, + units::meters_per_second_t{ + m_drive.GetRearLeftEncoder().GetRate()}, + units::meters_per_second_t{ + m_drive.GetRearRightEncoder().GetRate()}}; + }, + + frc::PIDController{DriveConstants::kPFrontLeftVel, 0, 0}, + frc::PIDController{DriveConstants::kPRearLeftVel, 0, 0}, + frc::PIDController{DriveConstants::kPFrontRightVel, 0, 0}, + frc::PIDController{DriveConstants::kPRearRightVel, 0, 0}, + + [this](units::volt_t frontLeft, units::volt_t rearLeft, + units::volt_t frontRight, units::volt_t rearRight) { + m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight, + rearRight); + }, + + {&m_drive}) + .ToPtr(); + + return frc2::cmd::Sequence( + std::move(mecanumDriveCommand), + frc2::InstantCommand([this] { m_drive.Drive(0, 0, 0, false); }, {}) + .ToPtr()); } diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h index 93485e3bb0f..c6f8afc703c 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -28,7 +29,7 @@ class RobotContainer { public: RobotContainer(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller @@ -44,8 +45,5 @@ class RobotContainer { frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, {}}; - // The chooser for the autonomous routines - frc::SendableChooser m_chooser; - void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp index 69895cb1cae..df5ba8ce95e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp @@ -4,9 +4,17 @@ #include "RobotContainer.h" +#include + RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here + m_chooser.SetDefaultOption("ONE", CommandSelector::ONE); + m_chooser.AddOption("TWO", CommandSelector::TWO); + m_chooser.AddOption("THREE", CommandSelector::THREE); + + frc::SmartDashboard::PutData("Auto Chooser", &m_chooser); + // Configure the button bindings ConfigureButtonBindings(); } diff --git a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h index a1a2c86fac7..ce5e26d679e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h @@ -4,7 +4,9 @@ #pragma once +#include #include +#include #include /** @@ -24,10 +26,8 @@ class RobotContainer { // The enum used as keys for selecting the command to run. enum CommandSelector { ONE, TWO, THREE }; - // An example selector method for the selectcommand. Returns the selector - // that will select which command to run. Can base this choice on logical - // conditions evaluated at runtime. - CommandSelector Select() { return ONE; } + // An example of how command selector may be used with SendableChooser + frc::SendableChooser m_chooser; // The robot's subsystems and commands are defined here... @@ -36,7 +36,7 @@ class RobotContainer { // takes a generic type, so the selector does not have to be an enum; it could // be any desired type (string, integer, boolean, double...) frc2::CommandPtr m_exampleSelectCommand = frc2::cmd::Select( - [this] { return Select(); }, + [this] { return m_chooser.GetSelected(); }, // Maps selector values to commands std::pair{ONE, frc2::cmd::Print("Command one was selected!")}, std::pair{TWO, frc2::cmd::Print("Command two was selected!")}, diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp index c7eab485a5d..c4c1661d699 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp @@ -37,7 +37,7 @@ void Robot::DisabledPeriodic() {} void Robot::AutonomousInit() { m_autonomousCommand = m_container.GetAutonomousCommand(); - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Schedule(); } } @@ -49,9 +49,9 @@ void Robot::TeleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != nullptr) { + if (m_autonomousCommand) { m_autonomousCommand->Cancel(); - m_autonomousCommand = nullptr; + m_autonomousCommand.reset(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp index 4bbbe0c4c94..9865e00eb50 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -48,7 +49,7 @@ RobotContainer::RobotContainer() { void RobotContainer::ConfigureButtonBindings() {} -frc2::Command* RobotContainer::GetAutonomousCommand() { +frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // Set up config for trajectory frc::TrajectoryConfig config(AutoConstants::kMaxSpeed, AutoConstants::kMaxAcceleration); @@ -73,24 +74,27 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi}, units::radian_t{std::numbers::pi}); - frc2::SwerveControllerCommand<4> swerveControllerCommand( - exampleTrajectory, [this]() { return m_drive.GetPose(); }, + // Reset odometry to the starting pose of the trajectory. + m_drive.ResetOdometry(exampleTrajectory.InitialPose()); - m_drive.kDriveKinematics, + frc2::CommandPtr swerveDriveCommand = + frc2::SwerveControllerCommand<4>( + exampleTrajectory, [this]() { return m_drive.GetPose(); }, - frc::PIDController{AutoConstants::kPXController, 0, 0}, - frc::PIDController{AutoConstants::kPYController, 0, 0}, thetaController, + m_drive.kDriveKinematics, - [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); }, + frc::PIDController{AutoConstants::kPXController, 0, 0}, + frc::PIDController{AutoConstants::kPYController, 0, 0}, + thetaController, - {&m_drive}); + [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); }, - // Reset odometry to the starting pose of the trajectory. - m_drive.ResetOdometry(exampleTrajectory.InitialPose()); + {&m_drive}) + .ToPtr(); - // no auto - return new frc2::SequentialCommandGroup( - std::move(swerveControllerCommand), + return frc2::cmd::Sequence( + std::move(swerveDriveCommand), frc2::InstantCommand( - [this]() { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})); + [this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {}) + .ToPtr()); } diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h index a82f2ac63b0..de9c814e752 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include @@ -24,7 +26,7 @@ class Robot : public frc::TimedRobot { private: // Have it null by default so that if testing teleop it // doesn't have undefined behavior and potentially crash. - frc2::Command* m_autonomousCommand = nullptr; + std::optional m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h index 3466121ab6a..4c7916456ab 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -28,7 +29,7 @@ class RobotContainer { public: RobotContainer(); - frc2::Command* GetAutonomousCommand(); + frc2::CommandPtr GetAutonomousCommand(); private: // The driver's controller @@ -39,8 +40,5 @@ class RobotContainer { // The robot's subsystems DriveSubsystem m_drive; - // The chooser for the autonomous routines - frc::SendableChooser m_chooser; - void ConfigureButtonBindings(); };