[commands] Add convenience factories (#4460)

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

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};
+};