[commands] Add convenience factories (#4460)

Co-authored-by: Starlight220 <53231611+Starlight220@users.noreply.github.com>

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/RapidReactCommandBot.cpp

@@ -0,0 +1,50 @@
+// 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.
+
+#include "RapidReactCommandBot.h"
+
+#include <frc2/command/Command.h>
+#include <frc2/command/Commands.h>
+#include <frc2/command/button/Trigger.h>
+
+#include "Constants.h"
+
+void RapidReactCommandBot::ConfigureBindings() {
+  // Automatically run the storage motor whenever the ball storage is not full,
+  // and turn it off whenever it fills.
+  frc2::Trigger([this] {
+    return m_storage.IsFull();
+  }).WhileFalse(m_storage.RunCommand());
+
+  // Automatically disable and retract the intake whenever the ball storage is
+  // full.
+  frc2::Trigger([this] {
+    return m_storage.IsFull();
+  }).OnTrue(m_intake.RetractCommand());
+
+  // Control the drive with split-stick arcade controls
+  m_drive.SetDefaultCommand(m_drive.ArcadeDriveCommand(
+      [this] { return m_driverController.GetLeftY(); },
+      [this] { return m_driverController.GetRightX(); }));
+
+  // Deploy the intake with the X button
+  m_driverController.X().OnTrue(m_intake.IntakeCommand());
+  // Retract the intake with the Y button
+  m_driverController.Y().OnTrue(m_intake.RetractCommand());
+
+  // Fire the shooter with the A button
+  m_driverController.A().OnTrue(
+      frc2::cmd::Parallel(
+          m_shooter.ShootCommand(ShooterConstants::kShooterTarget),
+          m_storage.RunCommand())
+          // Since we composed this inline we should give it a name
+          .WithName("Shoot"));
+}
+
+frc2::CommandPtr RapidReactCommandBot::GetAutonomousCommand() {
+  return m_drive
+      .DriveDistanceCommand(AutoConstants::kDriveDistance,
+                            AutoConstants::kDriveSpeed)
+      .WithTimeout(AutoConstants::kTimeout);
+}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/Robot.cpp

@@ -0,0 +1,58 @@
+// 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.
+
+#include "Robot.h"
+
+void Robot::RobotInit() {
+  // Configure default commands and condition bindings on robot startup
+  m_robot.ConfigureBindings();
+}
+
+void Robot::RobotPeriodic() {
+  // Runs the Scheduler.  This is responsible for polling buttons, adding
+  // newly-scheduled commands, running already-scheduled commands, removing
+  // finished or interrupted commands, and running subsystem Periodic() methods.
+  // This must be called from the robot's periodic block in order for anything
+  // in the Command-based framework to work.
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_robot.GetAutonomousCommand();
+
+  if (m_autonomousCommand) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // 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) {
+    m_autonomousCommand->Cancel();
+  }
+}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TestInit() {
+  // Cancels all running commands at the start of test mode.
+  frc2::CommandScheduler::GetInstance().CancelAll();
+}
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp

@@ -0,0 +1,46 @@
+// 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.
+
+#include "subsystems/Drive.h"
+
+#include <utility>
+
+#include <frc2/command/Commands.h>
+
+Drive::Drive() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
+  // Sets the distance per pulse for the encoders
+  m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
+  m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
+}
+
+frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
+                                           std::function<double()> rot) {
+  return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
+           m_drive.ArcadeDrive(fwd(), rot());
+         })
+      .WithName("ArcadeDrive");
+}
+
+frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
+                                             double speed) {
+  return RunOnce([this] {
+           // Reset encoders at the start of the command
+           m_leftEncoder.Reset();
+           m_rightEncoder.Reset();
+         })
+      // Drive forward at specified speed
+      .AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); }))
+      .Until([this, distance] {
+        return units::math::max(units::meter_t(m_leftEncoder.GetDistance()),
+                                units::meter_t(m_rightEncoder.GetDistance())) >=
+               distance;
+      })
+      // Stop the drive when the command ends
+      .FinallyDo([this](bool interrupted) { m_drive.StopMotor(); });
+}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Intake.cpp

@@ -0,0 +1,19 @@
+// 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.
+
+#include "subsystems/Intake.h"
+
+frc2::CommandPtr Intake::IntakeCommand() {
+  return RunOnce([this] { m_piston.Set(frc::DoubleSolenoid::kForward); })
+      .AndThen(Run([this] { m_motor.Set(1.0); }))
+      .WithName("Intake");
+}
+
+frc2::CommandPtr Intake::RetractCommand() {
+  return RunOnce([this] {
+           m_motor.Disable();
+           m_piston.Set(frc::DoubleSolenoid::kReverse);
+         })
+      .WithName("Retract");
+}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Shooter.cpp

@@ -0,0 +1,38 @@
+// 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.
+
+#include "subsystems/Shooter.h"
+
+#include <frc2/command/Commands.h>
+
+Shooter::Shooter() {
+  m_shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value());
+  m_shooterEncoder.SetDistancePerPulse(
+      ShooterConstants::kEncoderDistancePerPulse);
+
+  SetDefaultCommand(RunOnce([this] {
+                      m_shooterMotor.Disable();
+                      m_feederMotor.Disable();
+                    })
+                        .AndThen(Run([] {}))
+                        .WithName("Idle"));
+}
+
+frc2::CommandPtr Shooter::ShootCommand(units::turns_per_second_t setpoint) {
+  return frc2::cmd::Parallel(
+             // Run the shooter flywheel at the desired setpoint using
+             // feedforward and feedback
+             Run([this, setpoint] {
+               m_shooterMotor.SetVoltage(
+                   m_shooterFeedforward.Calculate(setpoint) +
+                   units::volt_t(m_shooterFeedback.Calculate(
+                       m_shooterEncoder.GetRate(), setpoint.value())));
+             }),
+             // Wait until the shooter has reached the setpoint, and then
+             // run the feeder
+             frc2::cmd::WaitUntil([this] {
+               return m_shooterFeedback.AtSetpoint();
+             }).AndThen([this] { m_feederMotor.Set(1.0); }))
+      .WithName("Shoot");
+}

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Storage.cpp

@@ -0,0 +1,18 @@
+// 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.
+
+#include "subsystems/Storage.h"
+
+Storage::Storage() {
+  SetDefaultCommand(
+      RunOnce([this] { m_motor.Disable(); }).AndThen([] {}).WithName("Idle"));
+}
+
+frc2::CommandPtr Storage::RunCommand() {
+  return Run([this] { m_motor.Set(1.0); }).WithName("Run");
+}
+
+bool Storage::IsFull() const {
+  return m_ballSensor.Get();
+}

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

@@ -0,0 +1,79 @@
+// 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 <numbers>
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace DriveConstants {
+constexpr int kLeftMotor1Port = 0;
+constexpr int kLeftMotor2Port = 1;
+constexpr int kRightMotor1Port = 2;
+constexpr int kRightMotor2Port = 3;
+
+constexpr int kLeftEncoderPorts[]{0, 1};
+constexpr int kRightEncoderPorts[]{2, 3};
+constexpr bool kLeftEncoderReversed = false;
+constexpr bool kRightEncoderReversed = true;
+
+constexpr double kEncoderCPR = 1024;
+constexpr units::meter_t kWheelDiameter = 6.0_in;
+constexpr double kEncoderDistancePerPulse =
+    // Assumes the encoders are directly mounted on the wheel shafts
+    ((kWheelDiameter * std::numbers::pi) / kEncoderCPR).value();
+
+}  // namespace DriveConstants
+
+namespace IntakeConstants {
+constexpr int kMotorPort = 6;
+constexpr int kSolenoidPorts[]{0, 1};
+}  // namespace IntakeConstants
+
+namespace StorageConstants {
+constexpr int kMotorPort = 7;
+constexpr int kBallSensorPort = 6;
+}  // namespace StorageConstants
+
+namespace ShooterConstants {
+constexpr int kEncoderPorts[]{4, 5};
+constexpr bool kEncoderReversed = false;
+constexpr double kEncoderCPR = 1024;
+constexpr double kEncoderDistancePerPulse =
+    // Distance units will be rotations
+    1.0 / kEncoderCPR;
+
+constexpr int kShooterMotorPort = 4;
+constexpr int kFeederMotorPort = 5;
+
+constexpr auto kShooterFree = 5300_tps;
+constexpr auto kShooterTarget = 4000_tps;
+constexpr auto kShooterTolerance = 50_tps;
+
+// These are not real PID gains, and will have to be tuned for your specific
+// robot.
+constexpr double kP = 1;
+
+constexpr units::volt_t kS = 0.05_V;
+constexpr auto kV =
+    // Should have value 12V at free speed...
+    12.0_V / kShooterFree;
+
+constexpr double kFeederSpeed = 0.5;
+}  // namespace ShooterConstants
+
+namespace OIConstants {
+constexpr int kDriverControllerPort = 0;
+}  // namespace OIConstants
+
+namespace AutoConstants {
+constexpr units::second_t kTimeout = 3.0_s;
+constexpr units::meter_t kDriveDistance = 2.0_m;
+constexpr double kDriveSpeed = 0.5;
+}  // namespace AutoConstants

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/RapidReactCommandBot.h

@@ -0,0 +1,52 @@
+// 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 <frc2/command/CommandPtr.h>
+#include <frc2/command/button/CommandXboxController.h>
+
+#include "Constants.h"
+#include "subsystems/Drive.h"
+#include "subsystems/Intake.h"
+#include "subsystems/Shooter.h"
+#include "subsystems/Storage.h"
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RapidReactCommandBot {
+ public:
+  /**
+   * Use this method to define bindings between conditions and commands. These
+   * are useful for automating robot behaviors based on button and sensor input.
+   *
+   * <p>Should be called during Robot::RobotInit().
+   *
+   * <p>Event binding methods are available on the frc2::Trigger class.
+   */
+  void ConfigureBindings();
+
+  /**
+   * Use this to define the command that runs during autonomous.
+   *
+   * <p>Scheduled during Robot::AutonomousInit().
+   */
+  frc2::CommandPtr GetAutonomousCommand();
+
+ private:
+  // The robot's subsystems
+  Drive m_drive;
+  Intake m_intake;
+  Shooter m_shooter;
+  Storage m_storage;
+
+  // The driver's controller
+  frc2::CommandXboxController m_driverController{
+      OIConstants::kDriverControllerPort};
+};

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

@@ -0,0 +1,30 @@
+// 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 <optional>
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/CommandPtr.h>
+
+#include "RapidReactCommandBot.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestInit() override;
+  void TestPeriodic() override;
+
+ private:
+  RapidReactCommandBot m_robot;
+  std::optional<frc2::CommandPtr> m_autonomousCommand;
+};

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h

@@ -0,0 +1,58 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/length.h>
+
+#include "Constants.h"
+
+class Drive : public frc2::SubsystemBase {
+ public:
+  Drive();
+  /**
+   * Returns a command that drives the robot with arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  [[nodiscard]] frc2::CommandPtr ArcadeDriveCommand(
+      std::function<double()> fwd, std::function<double()> rot);
+
+  /**
+   * Returns a command that drives the robot forward a specified distance at a
+   * specified speed.
+   *
+   * @param distance The distance to drive forward in meters
+   * @param speed The fraction of max speed at which to drive
+   */
+  [[nodiscard]] frc2::CommandPtr DriveDistanceCommand(units::meter_t distance,
+                                                      double speed);
+
+ private:
+  frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
+  frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
+  frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
+  frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
+
+  frc::MotorControllerGroup m_leftMotors{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightMotors{m_rightLeader, m_rightFollower};
+
+  frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
+
+  frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
+                             DriveConstants::kLeftEncoderPorts[1],
+                             DriveConstants::kLeftEncoderReversed};
+  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
+                              DriveConstants::kRightEncoderPorts[1],
+                              DriveConstants::kRightEncoderReversed};
+};

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Intake.h

@@ -0,0 +1,32 @@
+// 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 <functional>
+
+#include <frc/DoubleSolenoid.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class Intake : public frc2::SubsystemBase {
+ public:
+  Intake() = default;
+
+  /** Returns a command that deploys the intake, and then runs the intake motor
+   * indefinitely. */
+  [[nodiscard]] frc2::CommandPtr IntakeCommand();
+
+  /** Returns a command that turns off and retracts the intake. */
+  [[nodiscard]] frc2::CommandPtr RetractCommand();
+
+ private:
+  frc::PWMSparkMax m_motor{IntakeConstants::kMotorPort};
+  frc::DoubleSolenoid m_piston{frc::PneumaticsModuleType::REVPH,
+                               IntakeConstants::kSolenoidPorts[0],
+                               IntakeConstants::kSolenoidPorts[1]};
+};

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Shooter.h

@@ -0,0 +1,44 @@
+// 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 <functional>
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+#include "Constants.h"
+
+class Shooter : public frc2::SubsystemBase {
+ public:
+  Shooter();
+
+  /**
+   * Returns a command to shoot the balls currently stored in the robot. Spins
+   * the shooter flywheel up to the specified setpoint, and then runs the feeder
+   * motor.
+   *
+   * @param setpointRotationsPerSecond The desired shooter velocity
+   */
+  [[nodiscard]] frc2::CommandPtr ShootCommand(
+      units::turns_per_second_t setpoint);
+
+ private:
+  frc::PWMSparkMax m_shooterMotor{ShooterConstants::kShooterMotorPort};
+  frc::PWMSparkMax m_feederMotor{ShooterConstants::kFeederMotorPort};
+
+  frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0],
+                                ShooterConstants::kEncoderPorts[1],
+                                ShooterConstants::kEncoderReversed};
+  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward{
+      ShooterConstants::kS, ShooterConstants::kV};
+  frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
+};

wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Storage.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 <frc/DigitalInput.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/CommandPtr.h>
+#include <frc2/command/SubsystemBase.h>
+
+#include "Constants.h"
+
+class Storage : frc2::SubsystemBase {
+ public:
+  Storage();
+  /** Returns a command that runs the storage motor indefinitely. */
+  [[nodiscard]] frc2::CommandPtr RunCommand();
+
+  /** Whether the ball storage is full. */
+  bool IsFull() const;
+
+ private:
+  frc::PWMSparkMax m_motor{StorageConstants::kMotorPort};
+  frc::DigitalInput m_ballSensor{StorageConstants::kBallSensorPort};
+};

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -356,6 +356,18 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name": "Rapid React Command Bot",
+    "description": "A fully-functional command-based fender bot for the 2022 game using the new command framework.",
+    "tags": [
+      "Complete robot",
+      "Command-based",
+      "Lambdas"
+    ],
+    "foldername": "RapidReactCommandBot",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
   {
     "name": "Select Command Example",
     "description": "An example showing how to use the SelectCommand class from the new command framework.",