[examples] C++ RamseteCommand: Fix units (#4954)

Also fix the memory leak in the command-based auto.

wpilibcExamples/src/main/cpp/examples/RamseteCommand/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/RamseteCommand/cpp/RobotContainer.cpp

@@ -12,9 +12,8 @@
 #include <frc/trajectory/Trajectory.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
 #include <frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h>
-#include <frc2/command/InstantCommand.h>
+#include <frc2/command/Commands.h>
 #include <frc2/command/RamseteCommand.h>
-#include <frc2/command/SequentialCommandGroup.h>
 #include <frc2/command/button/JoystickButton.h>
 
 #include "Constants.h"
@@ -26,7 +25,7 @@ RobotContainer::RobotContainer() {
   ConfigureButtonBindings();
 
   // Set up default drive command
-  m_drive.SetDefaultCommand(frc2::RunCommand(
+  m_drive.SetDefaultCommand(frc2::cmd::Run(
       [this] {
         m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
                             -m_driverController.GetRightX());
@@ -43,7 +42,7 @@ void RobotContainer::ConfigureButtonBindings() {
       .OnFalse(&m_driveFullSpeed);
 }
 
-frc2::Command* RobotContainer::GetAutonomousCommand() {
+frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // Create a voltage constraint to ensure we don't accelerate too fast
   frc::DifferentialDriveVoltageConstraint autoVoltageConstraint{
       frc::SimpleMotorFeedforward<units::meters>{
@@ -69,9 +68,8 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
       // Pass the config
       config);
 
-  frc2::RamseteCommand ramseteCommand{
-      exampleTrajectory,
-      [this]() { return m_drive.GetPose(); },
+  frc2::CommandPtr ramseteCommand{frc2::RamseteCommand(
+      exampleTrajectory, [this] { return m_drive.GetPose(); },
       frc::RamseteController{AutoConstants::kRamseteB,
                              AutoConstants::kRamseteZeta},
       frc::SimpleMotorFeedforward<units::meters>{
@@ -81,13 +79,12 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
       frc2::PIDController{DriveConstants::kPDriveVel, 0, 0},
       frc2::PIDController{DriveConstants::kPDriveVel, 0, 0},
       [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
-      {&m_drive}};
+      {&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),
-      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
+  return std::move(ramseteCommand)
+      .BeforeStarting(
+          frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
 }

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -23,8 +23,8 @@ DriveSubsystem::DriveSubsystem()
   m_rightMotors.SetInverted(true);
 
   // Set the distance per pulse for the encoders
-  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
-  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
+  m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
+  m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
 
   ResetEncoders();
 }

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h

@@ -39,11 +39,10 @@ constexpr auto kTrackwidth = 0.69_m;
 extern const frc::DifferentialDriveKinematics kDriveKinematics;
 
 constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterInches = 6;
-constexpr double kEncoderDistancePerPulse =
+constexpr units::meter_t kWheelDiameter = 6_in;
+constexpr auto kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * std::numbers::pi) /
-    static_cast<double>(kEncoderCPR);
+    (kWheelDiameter * std::numbers::pi) / static_cast<double>(kEncoderCPR);
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or

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

@@ -4,8 +4,10 @@
 
 #pragma once
 
+#include <optional>
+
 #include <frc/TimedRobot.h>
-#include <frc2/command/Command.h>
+#include <frc2/command/CommandPtr.h>
 
 #include "RobotContainer.h"
 
@@ -22,9 +24,8 @@ class Robot : public frc::TimedRobot {
   void TestPeriodic() override;
 
  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;
+  // Have it empty by default
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
 
   RobotContainer m_container;
 };

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

@@ -7,11 +7,8 @@
 #include <frc/XboxController.h>
 #include <frc/controller/PIDController.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>
-#include <frc2/command/RunCommand.h>
 
 #include "Constants.h"
 #include "subsystems/DriveSubsystem.h"
@@ -27,7 +24,7 @@ class RobotContainer {
  public:
   RobotContainer();
 
-  frc2::Command* GetAutonomousCommand();
+  frc2::CommandPtr GetAutonomousCommand();
 
  private:
   // The driver's controller
@@ -38,13 +35,11 @@ class RobotContainer {
   // The robot's subsystems
   DriveSubsystem m_drive;
 
+  // RobotContainer-owned commands
   frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
                                         {}};
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
 
-  // The chooser for the autonomous routines
-  frc::SendableChooser<frc2::Command*> m_chooser;
-
   void ConfigureButtonBindings();
 };