diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp index 1593f225ff..4dbff4aa29 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp @@ -7,10 +7,11 @@ #include #include #include -#include #include #include +namespace ac = AutoConstants; + RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here @@ -18,7 +19,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()); @@ -30,32 +31,29 @@ void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA) - .OnTrue(frc2::cmd::RunOnce( - [this] { - m_arm.SetGoal(2_rad); - m_arm.Enable(); - }, - {&m_arm})); + m_driverController.A().OnTrue(frc2::cmd::RunOnce( + [this] { + m_arm.SetGoal(2_rad); + m_arm.Enable(); + }, + {&m_arm})); // Move the arm to neutral position when the 'B' button is pressed. - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB) - .OnTrue(frc2::cmd::RunOnce( - [this] { - m_arm.SetGoal(ArmConstants::kArmOffset); - m_arm.Enable(); - }, - {&m_arm})); + m_driverController.B().OnTrue(frc2::cmd::RunOnce( + [this] { + m_arm.SetGoal(ArmConstants::kArmOffset); + m_arm.Enable(); + }, + {&m_arm})); // Disable the arm controller when Y is pressed. - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kY) - .OnTrue(frc2::cmd::RunOnce([this] { m_arm.Disable(); }, {&m_arm})); + m_driverController.Y().OnTrue( + frc2::cmd::RunOnce([this] { m_arm.Disable(); }, {&m_arm})); // While holding the shoulder button, drive at half speed - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) + m_driverController.RightBumper() .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); })) - .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); })); + .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); })); } void RobotContainer::DisablePIDSubsystems() { @@ -63,6 +61,5 @@ void RobotContainer::DisablePIDSubsystems() { } frc2::Command* RobotContainer::GetAutonomousCommand() { - // Runs the chosen command in autonomous - return new frc2::InstantCommand([] {}); + return nullptr; } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h index 5d2c148227..67d194c694 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h @@ -4,16 +4,13 @@ #pragma once -#include -#include #include +#include #include "Constants.h" #include "subsystems/ArmSubsystem.h" #include "subsystems/DriveSubsystem.h" -namespace ac = AutoConstants; - /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should @@ -35,7 +32,8 @@ class RobotContainer { private: // The driver's controller - frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + frc2::CommandXboxController m_driverController{ + OIConstants::kDriverControllerPort}; // The robot's subsystems and commands are defined here... @@ -43,7 +41,5 @@ class RobotContainer { DriveSubsystem m_drive; ArmSubsystem m_arm; - // The chooser for the autonomous routines - void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp index f3ef220938..db1793e3d8 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp @@ -4,11 +4,6 @@ #include "RobotContainer.h" -#include -#include -#include -#include -#include #include RobotContainer::RobotContainer() { @@ -18,34 +13,27 @@ RobotContainer::RobotContainer() { ConfigureButtonBindings(); // Set up default drive command - m_drive.SetDefaultCommand(frc2::RunCommand( - [this] { - m_drive.ArcadeDrive(-m_driverController.GetLeftY(), - m_driverController.GetRightX()); - }, - {&m_drive})); + m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand( + [this] { return -m_driverController.GetLeftY(); }, + [this] { return m_driverController.GetRightX(); })); } void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA) - .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetGoal(2_rad); }, {&m_arm})); + m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad)); // Move the arm to neutral position when the 'B' button is pressed. - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB) - .OnTrue(frc2::cmd::RunOnce( - [this] { m_arm.SetGoal(ArmConstants::kArmOffset); }, {&m_arm})); + m_driverController.B().OnTrue( + m_arm.SetArmGoalCommand(ArmConstants::kArmOffset)); // While holding the shoulder button, drive at half speed - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) - .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); })) - .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); })); + m_driverController.RightBumper() + .OnTrue(m_drive.SetMaxOutputCommand(0.5)) + .OnFalse(m_drive.SetMaxOutputCommand(1.0)); } frc2::Command* RobotContainer::GetAutonomousCommand() { - // Runs the chosen command in autonomous - return new frc2::InstantCommand([] {}); + return nullptr; } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp index 7557b5ae58..5603c680a2 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp @@ -25,3 +25,7 @@ void ArmSubsystem::UseState(State setpoint) { m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, setpoint.position.value(), feedforward / 12_V); } + +frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) { + return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this}); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp index fac0df9493..16409ad7a7 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -26,10 +26,6 @@ void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. } -void DriveSubsystem::ArcadeDrive(double fwd, double rot) { - m_drive.ArcadeDrive(fwd, rot); -} - void DriveSubsystem::ResetEncoders() { m_leftEncoder.Reset(); m_rightEncoder.Reset(); @@ -47,6 +43,16 @@ frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; } -void DriveSubsystem::SetMaxOutput(double maxOutput) { - m_drive.SetMaxOutput(maxOutput); +frc2::CommandPtr DriveSubsystem::SetMaxOutputCommand(double maxOutput) { + return frc2::cmd::RunOnce( + [this, maxOutput] { m_drive.SetMaxOutput(maxOutput); }); +} + +frc2::CommandPtr DriveSubsystem::ArcadeDriveCommand( + std::function fwd, std::function rot) { + return frc2::cmd::Run( + [this, fwd = std::move(fwd), rot = std::move(rot)] { + m_drive.ArcadeDrive(fwd(), rot()); + }, + {this}); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h index 275230eb36..08a4dbea30 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h @@ -4,9 +4,8 @@ #pragma once -#include -#include #include +#include #include "Constants.h" #include "subsystems/ArmSubsystem.h" @@ -29,7 +28,8 @@ class RobotContainer { private: // The driver's controller - frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + frc2::CommandXboxController m_driverController{ + OIConstants::kDriverControllerPort}; // The robot's subsystems and commands are defined here... @@ -37,7 +37,5 @@ class RobotContainer { DriveSubsystem m_drive; ArmSubsystem m_arm; - // The chooser for the autonomous routines - void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h index 1fb5b873c7..ea2dc644e8 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include @@ -21,6 +22,8 @@ class ArmSubsystem : public frc2::TrapezoidProfileSubsystem { void UseState(State setpoint) override; + frc2::CommandPtr SetArmGoalCommand(units::radian_t goal); + private: ExampleSmartMotorController m_motor; frc::ArmFeedforward m_feedforward; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h index 47bf28e4d9..6830b960d0 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h @@ -4,10 +4,13 @@ #pragma once +#include + #include #include #include #include +#include #include #include "Constants.h" @@ -23,14 +26,6 @@ class DriveSubsystem : public frc2::SubsystemBase { // Subsystem methods go here. - /** - * Drives the robot using arcade controls. - * - * @param fwd the commanded forward movement - * @param rot the commanded rotation - */ - void ArcadeDrive(double fwd, double rot); - /** * Resets the drive encoders to currently read a position of 0. */ @@ -63,7 +58,10 @@ class DriveSubsystem : public frc2::SubsystemBase { * * @param maxOutput the maximum output to which the drive will be constrained */ - void SetMaxOutput(double maxOutput); + frc2::CommandPtr SetMaxOutputCommand(double maxOutput); + + frc2::CommandPtr ArcadeDriveCommand(std::function fwd, + std::function rot); private: // Components (e.g. motor controllers and sensors) should generally be diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp index 7dc1ebf811..4311db02ae 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -4,10 +4,6 @@ #include "RobotContainer.h" -#include -#include -#include - #include "commands/DriveDistanceProfiled.h" RobotContainer::RobotContainer() { @@ -29,39 +25,38 @@ void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here // While holding the shoulder button, drive at half speed - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) - .OnTrue(&m_driveHalfSpeed) - .OnFalse(&m_driveFullSpeed); + m_driverController.RightBumper() + .OnTrue(m_driveHalfSpeed.get()) + .OnFalse(m_driveFullSpeed.get()); // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of // 10 seconds - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA) - .OnTrue(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s)); + m_driverController.A().OnTrue( + DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s)); // Do the same thing as above when the 'B' button is pressed, but defined // inline - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB) - .OnTrue( - frc2::TrapezoidProfileCommand( - frc::TrapezoidProfile( - // Limit the max acceleration and velocity - {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}, - // End at desired position in meters; implicitly starts at 0 - {3_m, 0_mps}), - // Pipe the profile state to the drive - [this](auto setpointState) { - m_drive.SetDriveStates(setpointState, setpointState); - }, - // Require the drive - {&m_drive}) - .ToPtr() - .BeforeStarting( - frc2::cmd::RunOnce([this]() { m_drive.ResetEncoders(); }, {})) - .WithTimeout(10_s)); + m_driverController.B().OnTrue( + frc2::TrapezoidProfileCommand( + frc::TrapezoidProfile( + // Limit the max acceleration and velocity + {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}, + // End at desired position in meters; implicitly starts at 0 + {3_m, 0_mps}), + // Pipe the profile state to the drive + [this](auto setpointState) { + m_drive.SetDriveStates(setpointState, setpointState); + }, + // Require the drive + {&m_drive}) + // Convert to CommandPtr + .ToPtr() + .BeforeStarting( + frc2::cmd::RunOnce([this]() { m_drive.ResetEncoders(); }, {})) + .WithTimeout(10_s)); } frc2::Command* RobotContainer::GetAutonomousCommand() { // Runs the chosen command in autonomous - return new frc2::InstantCommand([] {}); + return nullptr; } diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h index aced9e05db..4700a70564 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h @@ -4,14 +4,10 @@ #pragma once -#include -#include #include -#include -#include -#include -#include -#include +#include +#include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -31,17 +27,19 @@ class RobotContainer { private: // The driver's controller - frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; + frc2::CommandXboxController m_driverController{ + OIConstants::kDriverControllerPort}; // The robot's subsystems and commands are defined here... // The robot's subsystems DriveSubsystem m_drive; - frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); }, - {}}; - frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, - {}}; + // RobotContainer-owned commands + frc2::CommandPtr m_driveHalfSpeed = + frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}); + frc2::CommandPtr m_driveFullSpeed = + frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {}); void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp index 342209d4e6..aeb1ea40e5 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp @@ -4,9 +4,6 @@ #include "RobotContainer.h" -#include -#include - RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here @@ -14,7 +11,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()); @@ -25,24 +22,42 @@ RobotContainer::RobotContainer() { void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here + // We can bind commands while keeping their ownership in RobotContainer + // Spin up the shooter when the 'A' button is pressed - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA) - .OnTrue(&m_spinUpShooter); + m_driverController.A().OnTrue(m_spinUpShooter.get()); // Turn off the shooter when the 'B' button is pressed - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB) - .OnTrue(&m_stopShooter); + m_driverController.B().OnTrue(m_stopShooter.get()); - // Shoot when the 'X' button is held - frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kX) - .OnTrue(&m_shoot) - .OnFalse(&m_stopFeeder); + // We can also *move* command ownership to the scheduler + // Note that we won't be able to access these commands after moving them! + + // Shoots if the shooter wheel has reached the target speed + frc2::CommandPtr shoot = frc2::cmd::Either( + // Run the feeder + frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}), + // Do nothing + frc2::cmd::None(), + // Determine which of the above to do based on whether the shooter has + // reached the desired speed + [this] { return m_shooter.AtSetpoint(); }); + + frc2::CommandPtr stopFeeder = + frc2::cmd::RunOnce([this] { m_shooter.StopFeeder(); }, {&m_shooter}); + + // Shoot when the 'X' button is pressed + m_driverController.X() + .OnTrue(std::move(shoot)) + .OnFalse(std::move(stopFeeder)); + + // We can also define commands inline at the binding! + // (ownership will be passed to the scheduler) // While holding the shoulder button, drive at half speed - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) - .OnTrue(&m_driveHalfSpeed) - .OnFalse(&m_driveFullSpeed); + m_driverController.RightBumper() + .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {})) + .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {})); } frc2::Command* RobotContainer::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h index 4c97c006da..40091f1a77 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h @@ -4,16 +4,10 @@ #pragma once -#include -#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -32,65 +26,47 @@ class RobotContainer { public: RobotContainer(); + // The chooser for the autonomous routines frc2::Command* GetAutonomousCommand(); private: // The driver's controller - frc::XboxController m_driverController{OIConstants::kDriverControllerPort}; - - // The robot's subsystems and commands are defined here... + frc2::CommandXboxController m_driverController{ + OIConstants::kDriverControllerPort}; // The robot's subsystems DriveSubsystem m_drive; ShooterSubsystem m_shooter; - // A simple autonomous routine that shoots the loaded frisbees + // RobotContainer-owned commands + // (These variables will still be valid after binding, because we don't move + // ownership) + + frc2::CommandPtr m_spinUpShooter = + frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter}); + + frc2::CommandPtr m_stopShooter = + frc2::cmd::RunOnce([this] { m_shooter.Disable(); }, {&m_shooter}); + + // An autonomous routine that shoots the loaded frisbees frc2::CommandPtr m_autonomousCommand = - frc2::SequentialCommandGroup{ + frc2::cmd::Sequence( // Start the command by spinning up the shooter... - frc2::InstantCommand([this] { m_shooter.Enable(); }, {&m_shooter}), + frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter}), // Wait until the shooter is at speed before feeding the frisbees - frc2::WaitUntilCommand([this] { return m_shooter.AtSetpoint(); }), + frc2::cmd::WaitUntil([this] { return m_shooter.AtSetpoint(); }), // Start running the feeder - frc2::InstantCommand([this] { m_shooter.RunFeeder(); }, {&m_shooter}), + frc2::cmd::RunOnce([this] { m_shooter.RunFeeder(); }, {&m_shooter}), // Shoot for the specified time - frc2::WaitCommand(ac::kAutoShootTimeSeconds)} + frc2::cmd::Wait(ac::kAutoShootTimeSeconds)) // Add a timeout (will end the command if, for instance, the shooter // never gets up to speed) .WithTimeout(ac::kAutoTimeoutSeconds) // When the command ends, turn off the shooter and the feeder - .AndThen([this] { + .AndThen(frc2::cmd::RunOnce([this] { m_shooter.Disable(); m_shooter.StopFeeder(); - }); - - // Assorted commands to be bound to buttons - - frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); }, - {&m_shooter}}; - - frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); }, - {&m_shooter}}; - - // Shoots if the shooter wheel has reached the target speed - frc2::ConditionalCommand m_shoot{ - // Run the feeder - frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}}, - // Do nothing - frc2::InstantCommand(), - // Determine which of the above to do based on whether the shooter has - // reached the desired speed - [this] { return m_shooter.AtSetpoint(); }}; - - frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); }, - {&m_shooter}}; - - 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 + })); void ConfigureButtonBindings(); }; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp index dd46d183eb..23757f8ce4 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp @@ -5,14 +5,14 @@ #include "RobotContainer.h" #include -#include RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here // Add commands to the autonomous command chooser - m_chooser.SetDefaultOption("Simple Auto", &m_simpleAuto); - m_chooser.AddOption("Complex Auto", &m_complexAuto); + // Note that we do *not* move ownership into the chooser + m_chooser.SetDefaultOption("Simple Auto", m_simpleAuto.get()); + m_chooser.AddOption("Complex Auto", m_complexAuto.get()); // Put the chooser on the dashboard frc::Shuffleboard::GetTab("Autonomous").Add(m_chooser); @@ -21,7 +21,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()); @@ -33,15 +33,15 @@ void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here // Grab the hatch when the 'Circle' button is pressed. - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCircle) - .OnTrue(&m_grabHatch); + m_driverController.Circle().OnTrue( + frc2::cmd::RunOnce([this] { m_hatch.GrabHatch(); }, {&m_hatch})); // Release the hatch when the 'Square' button is pressed. - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare) - .OnTrue(&m_releaseHatch); + m_driverController.Square().OnTrue( + frc2::cmd::RunOnce([this] { m_hatch.ReleaseHatch(); }, {&m_hatch})); // While holding R1, drive at half speed - frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1) - .OnTrue(&m_driveHalfSpeed) - .OnFalse(&m_driveFullSpeed); + m_driverController.R1() + .OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {})) + .OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {})); } frc2::Command* RobotContainer::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/Autos.cpp similarity index 59% rename from wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp rename to wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/Autos.cpp index 0123d9d8a7..723bb03ba9 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/Autos.cpp @@ -2,16 +2,37 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include "commands/ComplexAuto.h" +#include "commands/Autos.h" +#include #include -#include -#include + +#include "Constants.h" using namespace AutoConstants; -ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { - AddCommands( +frc2::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) { + return frc2::FunctionalCommand( + // Reset encoders on command start + [drive] { drive->ResetEncoders(); }, + // Drive forward while the command is executing + [drive] { drive->ArcadeDrive(AutoConstants::kAutoDriveSpeed, 0); }, + // Stop driving at the end of the command + [drive](bool interrupted) { drive->ArcadeDrive(0, 0); }, + // End the command when the robot's driven distance exceeds the + // desired value + [drive] { + return drive->GetAverageEncoderDistance() >= + AutoConstants::kAutoDriveDistanceInches; + }, + // Requires the drive subsystem + {drive}) + .ToPtr(); +} + +frc2::CommandPtr autos::ComplexAuto(DriveSubsystem* drive, + HatchSubsystem* hatch) { + return frc2::cmd::Sequence( // Drive forward the specified distance frc2::FunctionalCommand( // Reset encoders on command start @@ -27,9 +48,10 @@ ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { kAutoDriveDistanceInches; }, // Requires the drive subsystem - {drive}), + {drive}) + .ToPtr(), // Release the hatch - frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}), + frc2::cmd::RunOnce([hatch] { hatch->ReleaseHatch(); }, {hatch}), // Drive backward the specified distance // Drive forward the specified distance frc2::FunctionalCommand( @@ -46,5 +68,6 @@ ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) { kAutoBackupDistanceInches; }, // Requires the drive subsystem - {drive})); + {drive}) + .ToPtr()); } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h index b17c55dbea..002a8a60cb 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h @@ -4,17 +4,13 @@ #pragma once -#include #include #include -#include -#include -#include -#include -#include +#include +#include #include "Constants.h" -#include "commands/ComplexAuto.h" +#include "commands/Autos.h" #include "subsystems/DriveSubsystem.h" #include "subsystems/HatchSubsystem.h" @@ -35,7 +31,8 @@ class RobotContainer { private: // The driver's controller - frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort}; + frc2::CommandPS4Controller m_driverController{ + OIConstants::kDriverControllerPort}; // The robot's subsystems and commands are defined here... @@ -43,33 +40,11 @@ class RobotContainer { DriveSubsystem m_drive; HatchSubsystem m_hatch; + // Commands owned by RobotContainer + // The autonomous routines - frc2::FunctionalCommand m_simpleAuto = frc2::FunctionalCommand( - // Reset encoders on command start - [this] { m_drive.ResetEncoders(); }, - // Drive forward while the command is executing - [this] { m_drive.ArcadeDrive(AutoConstants::kAutoDriveSpeed, 0); }, - // Stop driving at the end of the command - [this](bool interrupted) { m_drive.ArcadeDrive(0, 0); }, - // End the command when the robot's driven distance exceeds the desired - // value - [this] { - return m_drive.GetAverageEncoderDistance() >= - AutoConstants::kAutoDriveDistanceInches; - }, - // Requires the drive subsystem - {&m_drive}); - ComplexAuto m_complexAuto{&m_drive, &m_hatch}; - - // Assorted commands to be bound to buttons - - frc2::InstantCommand m_grabHatch{[this] { m_hatch.GrabHatch(); }, {&m_hatch}}; - frc2::InstantCommand m_releaseHatch{[this] { m_hatch.ReleaseHatch(); }, - {&m_hatch}}; - frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); }, - {}}; - frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); }, - {}}; + frc2::CommandPtr m_simpleAuto = autos::SimpleAuto(&m_drive); + frc2::CommandPtr m_complexAuto = autos::ComplexAuto(&m_drive, &m_hatch); // The chooser for the autonomous routines frc::SendableChooser m_chooser; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/Autos.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/Autos.h new file mode 100644 index 0000000000..8adbdf8b3f --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/Autos.h @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "subsystems/DriveSubsystem.h" +#include "subsystems/HatchSubsystem.h" + +/** Container for auto command factories. */ +namespace autos { + +/** + * A simple auto that drives forward, then stops. + */ +frc2::CommandPtr SimpleAuto(DriveSubsystem* drive); + +/** + * A complex auto command that drives forward, releases a hatch, and then drives + * backward. + */ +frc2::CommandPtr ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); + +} // namespace autos diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h deleted file mode 100644 index e746d8ae6d..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include - -#include "Constants.h" -#include "subsystems/DriveSubsystem.h" -#include "subsystems/HatchSubsystem.h" - -/** - * A complex auto command that drives forward, releases a hatch, and then drives - * backward. - */ -class ComplexAuto - : public frc2::CommandHelper { - public: - /** - * Creates a new ComplexAuto. - * - * @param drive The drive subsystem this command will run on - * @param hatch The hatch subsystem this command will run on - */ - ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp index 379afcb7a2..1bee5d9ddb 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp @@ -34,10 +34,8 @@ RobotContainer::RobotContainer() { void RobotContainer::ConfigureButtonBindings() { // Configure your button bindings here - // NOTE: Using `new` here will leak these commands if they are ever no longer - // needed. This is usually a non-issue as button-bindings tend to be permanent - // - however, if you wish to avoid this, the commands should be - // stack-allocated and declared as members of RobotContainer. + // NOTE: since we're binding a CommandPtr, command ownership here is moved to + // the scheduler thus, no memory leaks! // Grab the hatch when the 'A' button is pressed. frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA) diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java index c2c401d86e..08c1c4aa7c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java @@ -4,15 +4,13 @@ package edu.wpi.first.wpilibj.examples.armbot; -import static edu.wpi.first.wpilibj.XboxController.Button; - import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; /** @@ -27,7 +25,8 @@ public class RobotContainer { private final ArmSubsystem m_robotArm = new ArmSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -39,7 +38,7 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand( + Commands.run( () -> m_robotDrive.arcadeDrive( -m_driverController.getLeftY(), m_driverController.getRightX()), @@ -54,9 +53,10 @@ public class RobotContainer { */ private void configureButtonBindings() { // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - new JoystickButton(m_driverController, Button.kA.value) + m_driverController + .a() .onTrue( - new InstantCommand( + Commands.runOnce( () -> { m_robotArm.setGoal(2); m_robotArm.enable(); @@ -64,9 +64,10 @@ public class RobotContainer { m_robotArm)); // Move the arm to neutral position when the 'B' button is pressed. - new JoystickButton(m_driverController, Button.kB.value) + m_driverController + .b() .onTrue( - new InstantCommand( + Commands.runOnce( () -> { m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads); m_robotArm.enable(); @@ -74,13 +75,13 @@ public class RobotContainer { m_robotArm)); // Disable the arm controller when Y is pressed. - new JoystickButton(m_driverController, Button.kY.value) - .onTrue(new InstantCommand(m_robotArm::disable)); + m_driverController.y().onTrue(Commands.runOnce(m_robotArm::disable)); // Drive at half speed when the bumper is held - new JoystickButton(m_driverController, Button.kRightBumper.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); + m_driverController + .rightBumper() + .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5))) + .onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1.0))); } /** @@ -97,6 +98,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); + return Commands.none(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java index bd19e4e853..67be2227f5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java @@ -4,15 +4,13 @@ package edu.wpi.first.wpilibj.examples.armbotoffboard; -import static edu.wpi.first.wpilibj.XboxController.Button; - import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; /** @@ -27,7 +25,8 @@ public class RobotContainer { private final ArmSubsystem m_robotArm = new ArmSubsystem(); // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -39,11 +38,8 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand( - () -> - m_robotDrive.arcadeDrive( - -m_driverController.getLeftY(), m_driverController.getRightX()), - m_robotDrive)); + m_robotDrive.arcadeDriveCommand( + () -> -m_driverController.getLeftY(), () -> m_driverController.getRightX())); } /** @@ -54,19 +50,18 @@ public class RobotContainer { */ private void configureButtonBindings() { // Move the arm to 2 radians above horizontal when the 'A' button is pressed. - new JoystickButton(m_driverController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_robotArm.setGoal(2), m_robotArm)); + m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2)); // Move the arm to neutral position when the 'B' button is pressed. - new JoystickButton(m_driverController, Button.kB.value) - .onTrue( - new InstantCommand( - () -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm)); + m_driverController + .b() + .onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads)); // Drive at half speed when the bumper is held - new JoystickButton(m_driverController, Button.kRightBumper.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); + m_driverController + .rightBumper() + .onTrue(m_robotDrive.limitOutputCommand(0.5)) + .onFalse(m_robotDrive.limitOutputCommand(1)); } /** @@ -75,6 +70,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); + return Commands.none(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java index 9e70402735..e083484b5e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants; import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; /** A robot arm subsystem that moves with a motion profile. */ @@ -36,4 +38,8 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem { m_motor.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0); } + + public Command setArmGoalCommand(double kArmOffsetRads) { + return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); + } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java index 939bcc3237..e3c9159537 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java @@ -9,7 +9,10 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. @@ -54,13 +57,14 @@ public class DriveSubsystem extends SubsystemBase { } /** - * Drives the robot using arcade controls. + * A split-stick arcade command, with forward/backward controlled by the left hand, and turning + * controlled by the right. * - * @param fwd the commanded forward movement - * @param rot the commanded rotation + * @param fwd supplier for the commanded forward movement + * @param rot supplier for the commanded rotation */ - public void arcadeDrive(double fwd, double rot) { - m_drive.arcadeDrive(fwd, rot); + public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { + return Commands.run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()), this); } /** Resets the drive encoders to currently read a position of 0. */ @@ -101,7 +105,7 @@ public class DriveSubsystem extends SubsystemBase { * * @param maxOutput the maximum output to which the drive will be constrained */ - public void setMaxOutput(double maxOutput) { - m_drive.setMaxOutput(maxOutput); + public Command limitOutputCommand(double maxOutput) { + return Commands.runOnce(() -> m_drive.setMaxOutput(maxOutput)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java index edadde41b5..4e21ff591b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -4,8 +4,6 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; -import static edu.wpi.first.wpilibj.XboxController.Button; - import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; @@ -13,10 +11,9 @@ import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstant import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -28,8 +25,13 @@ public class RobotContainer { // The robot's subsystems private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + // Retained command references + private final Command m_driveFullSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(1)); + private final Command m_driveHalfSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)); + // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -41,7 +43,7 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand( + Commands.run( () -> m_robotDrive.arcadeDrive( -m_driverController.getLeftY(), m_driverController.getRightX()), @@ -52,15 +54,18 @@ public class RobotContainer { * Use this method to define your button->command mappings. Buttons can be created by * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * JoystickButton}. + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + // Drive at half speed when the bumper is held + m_driverController.rightBumper().onTrue(m_driveHalfSpeed).onFalse(m_driveFullSpeed); + // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds - new JoystickButton(m_driverController, Button.kA.value) - .onTrue(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10)); + m_driverController.a().onTrue(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10)); // Do the same thing as above when the 'B' button is pressed, but defined inline - new JoystickButton(m_driverController, Button.kB.value) + m_driverController + .b() .onTrue( new TrapezoidProfileCommand( new TrapezoidProfile( @@ -76,11 +81,6 @@ public class RobotContainer { m_robotDrive) .beforeStarting(m_robotDrive::resetEncoders) .withTimeout(10)); - - // Drive at half speed when the bumper is held - new JoystickButton(m_driverController, Button.kRightBumper.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); } /** @@ -89,6 +89,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); + return Commands.none(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java index 30a4a1aa26..d3db131480 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java @@ -4,20 +4,13 @@ package edu.wpi.first.wpilibj.examples.frisbeebot; -import static edu.wpi.first.wpilibj.XboxController.Button; - -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants; import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -30,29 +23,34 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - // A simple autonomous routine that shoots the loaded frisbees - private final Command m_autoCommand = - // Start the command by spinning up the shooter... - new InstantCommand(m_shooter::enable, m_shooter) - .andThen( + private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter); + private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter); + + // An autonomous routine that shoots the loaded frisbees + Command m_autonomousCommand = + Commands.sequence( + // Start the command by spinning up the shooter... + Commands.runOnce(m_shooter::enable, m_shooter), // Wait until the shooter is at speed before feeding the frisbees - new WaitUntilCommand(m_shooter::atSetpoint), + Commands.waitUntil(m_shooter::atSetpoint), // Start running the feeder - new InstantCommand(m_shooter::runFeeder, m_shooter), + Commands.runOnce(m_shooter::runFeeder, m_shooter), // Shoot for the specified time - new WaitCommand(AutoConstants.kAutoShootTimeSeconds)) - // Add a timeout (will end the command if, for instance, the shooter never gets up to - // speed) + Commands.waitSeconds(AutoConstants.kAutoShootTimeSeconds)) + // Add a timeout (will end the command if, for instance, the shooter + // never gets up to speed) .withTimeout(AutoConstants.kAutoTimeoutSeconds) // When the command ends, turn off the shooter and the feeder .andThen( - () -> { - m_shooter.disable(); - m_shooter.stopFeeder(); - }); + Commands.runOnce( + () -> { + m_shooter.disable(); + m_shooter.stopFeeder(); + })); // The driver's controller - XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort); + CommandXboxController m_driverController = + new CommandXboxController(OIConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -64,7 +62,7 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand( + Commands.run( () -> m_robotDrive.arcadeDrive( -m_driverController.getLeftY(), m_driverController.getRightX()), @@ -72,37 +70,47 @@ public class RobotContainer { } /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + * Use this method to define your button->command mappings. Buttons can be created via the button + * factories on {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID} or one of its + * subclasses ({@link edu.wpi.first.wpilibj2.command.button.CommandJoystick} or {@link + * CommandXboxController}). */ private void configureButtonBindings() { + // Configure your button bindings here + + // We can bind commands while retaining references to them in RobotContainer + // Spin up the shooter when the 'A' button is pressed - new JoystickButton(m_driverController, Button.kA.value) - .onTrue(new InstantCommand(m_shooter::enable, m_shooter)); + m_driverController.a().onTrue(m_spinUpShooter); // Turn off the shooter when the 'B' button is pressed - new JoystickButton(m_driverController, Button.kB.value) - .onTrue(new InstantCommand(m_shooter::disable, m_shooter)); + m_driverController.b().onTrue(m_stopShooter); - // Run the feeder when the 'X' button is held, but only if the shooter is at speed - new JoystickButton(m_driverController, Button.kX.value) - .onTrue( - new ConditionalCommand( - // Run the feeder - new InstantCommand(m_shooter::runFeeder, m_shooter), - // Do nothing - new InstantCommand(), - // Determine which of the above to do based on whether the shooter has reached the - // desired speed - m_shooter::atSetpoint)) - .onFalse(new InstantCommand(m_shooter::stopFeeder, m_shooter)); + // We can also write them as temporary variables outside the bindings - // Drive at half speed when the bumper is held - new JoystickButton(m_driverController, Button.kRightBumper.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); + // Shoots if the shooter wheel has reached the target speed + Command shoot = + Commands.either( + // Run the feeder + Commands.runOnce(m_shooter::runFeeder, m_shooter), + // Do nothing + Commands.none(), + // Determine which of the above to do based on whether the shooter has reached the + // desired speed + m_shooter::atSetpoint); + + Command stopFeeder = Commands.runOnce(m_shooter::stopFeeder, m_shooter); + + // Shoot when the 'X' button is pressed + m_driverController.x().onTrue(shoot).onFalse(stopFeeder); + + // We can also define commands inline at the binding! + + // While holding the shoulder button, drive at half speed + m_driverController + .rightBumper() + .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5))) + .onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1))); } /** @@ -111,6 +119,6 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return m_autoCommand; + return m_autonomousCommand; } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java index ba7a889895..8018a5d5a8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java @@ -4,21 +4,16 @@ package edu.wpi.first.wpilibj.examples.hatchbotinlined; -import static edu.wpi.first.wpilibj.PS4Controller.Button; - import edu.wpi.first.wpilibj.PS4Controller; -import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.Autos; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -31,30 +26,20 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem(); + // Retained command handles + // The autonomous routines - // A simple auto routine that drives forward a specified distance, and then stops. - private final Command m_simpleAuto = - new FunctionalCommand( - // Reset encoders on command start - m_robotDrive::resetEncoders, - // Drive forward while the command is executing - () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0), - // Stop driving at the end of the command - interrupt -> m_robotDrive.arcadeDrive(0, 0), - // End the command when the robot's driven distance exceeds the desired value - () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches, - // Require the drive subsystem - m_robotDrive); - + private final Command m_simpleAuto = Autos.simpleAuto(m_robotDrive); // A complex auto routine that drives forward, drops a hatch, and then drives backward. - private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem); + private final Command m_complexAuto = Autos.complexAuto(m_robotDrive, m_hatchSubsystem); // A chooser for autonomous commands SendableChooser m_chooser = new SendableChooser<>(); // The driver's controller - PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort); + CommandPS4Controller m_driverController = + new CommandPS4Controller(OIConstants.kDriverControllerPort); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -66,7 +51,7 @@ public class RobotContainer { m_robotDrive.setDefaultCommand( // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - new RunCommand( + Commands.run( () -> m_robotDrive.arcadeDrive( -m_driverController.getLeftY(), m_driverController.getRightX()), @@ -88,15 +73,18 @@ public class RobotContainer { */ private void configureButtonBindings() { // Grab the hatch when the Circle button is pressed. - new JoystickButton(m_driverController, Button.kCircle.value) - .onTrue(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem)); + m_driverController + .circle() + .onTrue(Commands.runOnce(m_hatchSubsystem::grabHatch, m_hatchSubsystem)); // Release the hatch when the Square button is pressed. - new JoystickButton(m_driverController, Button.kSquare.value) - .onTrue(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem)); + m_driverController + .square() + .onTrue(Commands.runOnce(m_hatchSubsystem::releaseHatch, m_hatchSubsystem)); // While holding R1, drive at half speed - new JoystickButton(m_driverController, Button.kR1.value) - .onTrue(new InstantCommand(() -> m_robotDrive.setMaxOutput(0.5))) - .onFalse(new InstantCommand(() -> m_robotDrive.setMaxOutput(1))); + m_driverController + .R1() + .onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5))) + .onFalse(Commands.runOnce(() -> m_robotDrive.setMaxOutput(1))); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/Autos.java similarity index 61% rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/Autos.java index 1faf45a1d1..7712fb1101 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/Autos.java @@ -7,20 +7,30 @@ package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands; import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -/** A complex auto command that drives forward, releases a hatch, and then drives backward. */ -public class ComplexAutoCommand extends SequentialCommandGroup { - /** - * Creates a new ComplexAutoCommand. - * - * @param driveSubsystem The drive subsystem this command will run on - * @param hatchSubsystem The hatch subsystem this command will run on - */ - public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) { - addCommands( +/** Container for auto command factories. */ +public final class Autos { + /** A simple auto routine that drives forward a specified distance, and then stops. */ + public static Command simpleAuto(DriveSubsystem drive) { + return new FunctionalCommand( + // Reset encoders on command start + drive::resetEncoders, + // Drive forward while the command is executing + () -> drive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0), + // Stop driving at the end of the command + interrupt -> drive.arcadeDrive(0, 0), + // End the command when the robot's driven distance exceeds the desired value + () -> drive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches, + // Require the drive subsystem + drive); + } + + /** A complex auto routine that drives forward, drops a hatch, and then drives backward. */ + public static Command complexAuto(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) { + return Commands.sequence( // Drive forward up to the front of the cargo ship new FunctionalCommand( // Reset encoders on command start @@ -37,7 +47,7 @@ public class ComplexAutoCommand extends SequentialCommandGroup { driveSubsystem), // Release the hatch - new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem), + Commands.runOnce(hatchSubsystem::releaseHatch, hatchSubsystem), // Drive backward the specified distance new FunctionalCommand( @@ -54,4 +64,8 @@ public class ComplexAutoCommand extends SequentialCommandGroup { // Require the drive subsystem driveSubsystem)); } + + private Autos() { + throw new UnsupportedOperationException("This is a utility class!"); + } }