[commands] Add convenience factories (#4460)

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

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -340,6 +340,19 @@
     "mainclass": "Main",
     "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": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Select Command Example",
     "description": "An example showing how to use the SelectCommand class from the new command framework.",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java

@@ -0,0 +1,83 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot;
+
+import edu.wpi.first.math.util.Units;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+  public static final class DriveConstants {
+    public static final int kLeftMotor1Port = 0;
+    public static final int kLeftMotor2Port = 1;
+    public static final int kRightMotor1Port = 2;
+    public static final int kRightMotor2Port = 3;
+
+    public static final int[] kLeftEncoderPorts = {0, 1};
+    public static final int[] kRightEncoderPorts = {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+  }
+
+  public static final class ShooterConstants {
+    public static final int[] kEncoderPorts = {4, 5};
+    public static final boolean kEncoderReversed = false;
+    public static final int kEncoderCPR = 1024;
+    public static final double kEncoderDistancePerPulse =
+        // Distance units will be rotations
+        1.0 / (double) kEncoderCPR;
+
+    public static final int kShooterMotorPort = 4;
+    public static final int kFeederMotorPort = 5;
+
+    public static final double kShooterFreeRPS = 5300;
+    public static final double kShooterTargetRPS = 4000;
+    public static final double kShooterToleranceRPS = 50;
+
+    // These are not real PID gains, and will have to be tuned for your specific robot.
+    public static final double kP = 1;
+
+    // On a real robot the feedforward constants should be empirically determined; these are
+    // reasonable guesses.
+    public static final double kSVolts = 0.05;
+    public static final double kVVoltSecondsPerRotation =
+        // Should have value 12V at free speed...
+        12.0 / kShooterFreeRPS;
+
+    public static final double kFeederSpeed = 0.5;
+  }
+
+  public static final class IntakeConstants {
+    public static final int kMotorPort = 6;
+    public static final int[] kSolenoidPorts = {0, 1};
+  }
+
+  public static final class StorageConstants {
+    public static final int kMotorPort = 7;
+    public static final int kBallSensorPort = 6;
+  }
+
+  public static final class AutoConstants {
+    public static final double kTimeoutSeconds = 3;
+    public static final double kDriveDistanceMeters = 2;
+    public static final double kDriveSpeed = 0.5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java

@@ -0,0 +1,84 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot;
+
+import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel;
+
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Drive;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Intake;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Shooter;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems.Storage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+/**
+ * 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.
+ */
+public class RapidReactCommandBot {
+  // The robot's subsystems
+  private final Drive m_drive = new Drive();
+  private final Intake m_intake = new Intake();
+  private final Storage m_storage = new Storage();
+  private final Shooter m_shooter = new Shooter();
+
+  // The driver's controller
+  CommandXboxController m_driverController =
+      new CommandXboxController(OIConstants.kDriverControllerPort);
+
+  /**
+   * 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 {@link Robot#robotInit()}.
+   *
+   * <p>Event binding methods are available on the {@link Trigger} class.
+   */
+  public void configureBindings() {
+    // Automatically run the storage motor whenever the ball storage is not full,
+    // and turn it off whenever it fills.
+    new Trigger(m_storage::isFull).whileFalse(m_storage.runCommand());
+
+    // Automatically disable and retract the intake whenever the ball storage is full.
+    new Trigger(m_storage::isFull).onTrue(m_intake.retractCommand());
+
+    // Control the drive with split-stick arcade controls
+    m_drive.setDefaultCommand(
+        m_drive.arcadeDriveCommand(m_driverController::getLeftY, 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(
+            parallel(
+                    m_shooter.shootCommand(ShooterConstants.kShooterTargetRPS),
+                    m_storage.runCommand())
+                // Since we composed this inline we should give it a name
+                .withName("Shoot"));
+  }
+
+  /**
+   * Use this to define the command that runs during autonomous.
+   *
+   * <p>Scheduled during {@link Robot#autonomousInit()}.
+   */
+  public Command getAutonomousCommand() {
+    // Drive forward for 2 meters at half speed with a 3 second timeout
+    return m_drive
+        .driveDistanceCommand(AutoConstants.kDriveDistanceMeters, AutoConstants.kDriveSpeed)
+        .withTimeout(AutoConstants.kTimeoutSeconds);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Robot.java

@@ -0,0 +1,92 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private final RapidReactCommandBot m_robot = new RapidReactCommandBot();
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Configure default commands and condition bindings on robot startup
+    m_robot.configureBindings();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void 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.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robot.getAutonomousCommand();
+
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void 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 != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java

@@ -0,0 +1,94 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.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.SubsystemBase;
+import java.util.function.DoubleSupplier;
+
+public class Drive extends SubsystemBase {
+  // The motors on the left side of the drive.
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
+
+  // The motors on the right side of the drive.
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
+
+  // The robot's drive
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
+
+  // The left-side drive encoder
+  private final Encoder m_leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder m_rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+
+  /** Creates a new Drive subsystem. */
+  public 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);
+  }
+
+  /**
+   * Returns a command that drives the robot with arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
+    // A split-stick arcade command, with forward/backward controlled by the left
+    // hand, and turning controlled by the right.
+    return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
+        .withName("arcadeDrive");
+  }
+
+  /**
+   * Returns a command that drives the robot forward a specified distance at a specified speed.
+   *
+   * @param distanceMeters The distance to drive forward in meters
+   * @param speed The fraction of max speed at which to drive
+   */
+  public Command driveDistanceCommand(double distanceMeters, double speed) {
+    return runOnce(
+            () -> {
+              // Reset encoders at the start of the command
+              m_leftEncoder.reset();
+              m_rightEncoder.reset();
+            })
+        // Drive forward at specified speed
+        .andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
+        // End command when we've traveled the specified distance
+        .until(
+            () ->
+                Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance())
+                    >= distanceMeters)
+        // Stop the drive when the command ends
+        .finallyDo(interrupted -> m_drive.stopMotor());
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Intake.java

@@ -0,0 +1,39 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
+
+import static edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.IntakeConstants;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Intake extends SubsystemBase {
+  private final PWMSparkMax m_motor = new PWMSparkMax(IntakeConstants.kMotorPort);
+  private final DoubleSolenoid m_pistons =
+      new DoubleSolenoid(
+          PneumaticsModuleType.REVPH,
+          IntakeConstants.kSolenoidPorts[1],
+          IntakeConstants.kSolenoidPorts[2]);
+
+  /** Returns a command that deploys the intake, and then runs the intake motor indefinitely. */
+  public Command intakeCommand() {
+    return runOnce(() -> m_pistons.set(DoubleSolenoid.Value.kForward))
+        .andThen(run(() -> m_motor.set(1.0)))
+        .withName("Intake");
+  }
+
+  /** Returns a command that turns off and retracts the intake. */
+  public Command retractCommand() {
+    return runOnce(
+            () -> {
+              m_motor.disable();
+              m_pistons.set(DoubleSolenoid.Value.kReverse);
+            })
+        .withName("Retract");
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java

@@ -0,0 +1,66 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
+
+import static edu.wpi.first.wpilibj2.command.Commands.parallel;
+import static edu.wpi.first.wpilibj2.command.Commands.waitUntil;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Shooter extends SubsystemBase {
+  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
+  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
+  private final Encoder m_shooterEncoder =
+      new Encoder(
+          ShooterConstants.kEncoderPorts[0],
+          ShooterConstants.kEncoderPorts[1],
+          ShooterConstants.kEncoderReversed);
+  private final SimpleMotorFeedforward m_shooterFeedforward =
+      new SimpleMotorFeedforward(
+          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
+  private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);
+
+  /** The shooter subsystem for the robot. */
+  public Shooter() {
+    m_shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS);
+    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
+
+    // Set default command to turn off both the shooter and feeder motors, and then idle
+    setDefaultCommand(
+        runOnce(
+                () -> {
+                  m_shooterMotor.disable();
+                  m_feederMotor.disable();
+                })
+            .andThen(run(() -> {}))
+            .withName("Idle"));
+  }
+
+  /**
+   * 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
+   */
+  public Command shootCommand(double setpointRotationsPerSecond) {
+    return parallel(
+            // Run the shooter flywheel at the desired setpoint using feedforward and feedback
+            run(
+                () ->
+                    m_shooterMotor.set(
+                        m_shooterFeedforward.calculate(setpointRotationsPerSecond)
+                            + m_shooterFeedback.calculate(
+                                m_shooterEncoder.getRate(), setpointRotationsPerSecond))),
+            // Wait until the shooter has reached the setpoint, and then run the feeder
+            waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1)))
+        .withName("Shoot");
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Storage.java

@@ -0,0 +1,33 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
+
+import static edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.StorageConstants;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Storage extends SubsystemBase {
+  private final PWMSparkMax m_motor = new PWMSparkMax(StorageConstants.kMotorPort);
+  private final DigitalInput m_ballSensor = new DigitalInput(StorageConstants.kBallSensorPort);
+
+  /** Create a new Storage subsystem. */
+  public Storage() {
+    // Set default command to turn off the storage motor and then idle
+    setDefaultCommand(runOnce(m_motor::disable).andThen(run(() -> {})).withName("Idle"));
+  }
+
+  /** Whether the ball storage is full. */
+  public boolean isFull() {
+    return m_ballSensor.get();
+  }
+
+  /** Returns a command that runs the storage motor indefinitely. */
+  public Command runCommand() {
+    return run(() -> m_motor.set(1)).withName("run");
+  }
+}