diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp index c7eab485a5..c4c1661d69 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp +++ b/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(); } } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index a72b1292eb..89fc349926 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -12,9 +12,8 @@ #include #include #include -#include +#include #include -#include #include #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{ @@ -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{ @@ -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); }, {})); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp index f8ec1db47b..0bc598e4e9 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/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(); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h index 18747d4eed..f7a061a4ab 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h +++ b/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(kEncoderCPR); + (kWheelDiameter * std::numbers::pi) / static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h index a82f2ac63b..889bfdf3fa 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h @@ -4,8 +4,10 @@ #pragma once +#include + #include -#include +#include #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 m_autonomousCommand; RobotContainer m_container; }; diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h index 3d6a36fa29..b01e6e242a 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h @@ -7,11 +7,8 @@ #include #include #include -#include +#include #include -#include -#include -#include #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 m_chooser; - void ConfigureButtonBindings(); };