[examples] Update C++ examples to use CommandPtr (#5988)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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();
   }
 }
 

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp

@@ -12,6 +12,7 @@
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h>
+#include <frc2/command/Commands.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/MecanumControllerCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
@@ -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<units::meters>(ks, kv, ka),
-      DriveConstants::kDriveKinematics,
-
-      frc::PIDController{AutoConstants::kPXController, 0, 0},
-      frc::PIDController{AutoConstants::kPYController, 0, 0},
-      frc::ProfiledPIDController<units::radians>(
-          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<units::meters>(ks, kv, ka),
+          DriveConstants::kDriveKinematics,
+
+          frc::PIDController{AutoConstants::kPXController, 0, 0},
+          frc::PIDController{AutoConstants::kPYController, 0, 0},
+          frc::ProfiledPIDController<units::radians>(
+              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());
 }

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
 #include <frc2/command/Command.h>
 
@@ -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<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h

@@ -9,6 +9,7 @@
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/PIDCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
@@ -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<frc2::Command*> m_chooser;
-
   void ConfigureButtonBindings();
 };