[cmd3] Add example and template projects for commands v3 and make hatchbot example idiomatic

wpilibcExamples/example_projects.bzl

@@ -1,6 +1,7 @@
 EXAMPLE_FOLDERS = [
     "ArcadeDriveGamepad",
     "ArmSimulation",
+    "Commandv3",
     "DifferentialDriveBot",
     "DifferentialDrivePoseEstimator",
     "DriveDistanceOffboard",
@@ -82,6 +83,8 @@ SNIPPET_FOLDERS = [
 TEMPLATE_FOLDERS = [
     "commandv2",
     "commandv2skeleton",
+    "commandv3",
+    "commandv3skeleton",
     "opmode",
     "robotbaseskeleton",
     "timed",

wpilibjExamples/src/main/java/org/wpilib/examples/examples.json

@@ -10,6 +10,22 @@
     "robotclass": "Robot",
     "commandversion": 2
   },
+  {
+    "name": "Commands v3 - REBUILT Robot",
+    "description": "A robot with a drivetrain, intake, and shooter subsystems controlled by the commands v3 framework",
+    "tags": [
+      "Complete Robot",
+      "Commandv3",
+      "DataLog",
+      "Joystick",
+      "Swerve Drive",
+      "OpMode"
+    ],
+    "foldername": "rebuiltcmdv3",
+    "gradlebase": "java",
+    "robotclass": "Robot",
+    "commandversion": 3
+  },
   {
     "name": "Encoder",
     "description": "View values from a quadrature encoder.",

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java

@@ -38,7 +38,7 @@ public static final class HatchConstants {
 
   public static final class AutoConstants {
     public static final double kAutoDriveDistanceInches = 60;
-    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoBackupDistanceInches = -20;
     public static final double kAutoDriveSpeed = 0.5;
   }
 

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java

@@ -6,8 +6,14 @@
 
 import org.wpilib.command3.Command;
 import org.wpilib.command3.Scheduler;
+import org.wpilib.command3.button.CommandGamepad;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.examples.hatchbotcmdv3.commands.Autos;
+import org.wpilib.examples.hatchbotcmdv3.mechanisms.DriveMechanism;
+import org.wpilib.examples.hatchbotcmdv3.mechanisms.HatchMechanism;
 import org.wpilib.framework.TimedRobot;
+import org.wpilib.smartdashboard.SendableChooser;
+import org.wpilib.smartdashboard.SmartDashboard;
 import org.wpilib.system.DataLogManager;
 
 /**
@@ -16,18 +22,41 @@
  * this project, you must also update the Main.java file in the project.
  */
 public class Robot extends TimedRobot {
-  private Command autonomousCommand;
+  /**
+   * A chooser for autonomous commands. Drivers can choose between different autonomous modes on a
+   * dashboard before the start of a match.
+   */
+  private final SendableChooser<Command> autonomousChooser = new SendableChooser<>();
+
+  // The driver's controller
+  private final CommandGamepad driverController =
+      new CommandGamepad(Constants.OIConstants.kDriverControllerPort);
 
-  private final RobotContainer robotContainer;
+  // The robot's mechanisms
+  private final DriveMechanism robotDrive = new DriveMechanism();
+  private final HatchMechanism hatchMechanism = new HatchMechanism();
 
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
   public Robot() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-    // autonomous chooser on the dashboard.
-    robotContainer = new RobotContainer();
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive with forward/backward controlled by
+    // the left hand, and turning controlled by the right.
+    robotDrive.setDefaultCommand(
+        robotDrive.arcadeDrive(
+            () -> -driverController.getLeftY(), () -> -driverController.getRightX()));
+
+    // Add commands to the autonomous command chooser
+    autonomousChooser.setDefaultOption("Simple Auto", Autos.simpleAuto(robotDrive));
+    autonomousChooser.addOption("Complex Auto", Autos.complexAuto(robotDrive, hatchMechanism));
+
+    // Put the chooser on the dashboard
+    SmartDashboard.putData("Autonomous", autonomousChooser);
 
     // Start recording to data log
     DataLogManager.start();
@@ -37,6 +66,19 @@ public Robot() {
     DriverStation.startDataLog(DataLogManager.getLog(), true);
   }
 
+  /** Use this method to define your button->command mappings. */
+  private void configureButtonBindings() {
+    // Grab the hatch when the Circle button is pressed.
+    driverController.faceRight().onTrue(hatchMechanism.grabHatchCommand());
+    // Release the hatch when the Square button is pressed.
+    driverController.faceLeft().onTrue(hatchMechanism.releaseHatchCommand());
+    // While holding R1, drive at half speed
+    driverController
+        .rightBumper()
+        .onTrue(Command.noRequirements(_ -> robotDrive.setMaxOutput(0.5)).named("Set half speed"))
+        .onFalse(Command.noRequirements(_ -> robotDrive.setMaxOutput(1)).named("Set full speed"));
+  }
+
   /**
    * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
    * that you want ran during disabled, autonomous, teleoperated and test.
@@ -46,10 +88,10 @@ public Robot() {
    */
   @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.
+    // Runs the scheduler.  This is responsible for polling buttons, running scheduled commands,
+    // and removing finished or interrupted commands.
+    // This must be called from the robot's periodic block in order for anything in the
+    // Command-based framework to work.
     Scheduler.getDefault().run();
   }
 
@@ -60,12 +102,14 @@ public void disabledInit() {}
   @Override
   public void disabledPeriodic() {}
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /** This autonomous runs the autonomous command selected by the {@link #autonomousChooser}. */
   @Override
   public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
+    Command autonomousCommand = autonomousChooser.getSelected();
 
-    // schedule the autonomous command (example)
+    // Schedule the autonomous command.
+    // Because we schedule this command in the autonomous mode, it will be automatically canceled
+    // when the autonomous mode ends.
     if (autonomousCommand != null) {
       Scheduler.getDefault().schedule(autonomousCommand);
     }
@@ -76,23 +120,15 @@ public void autonomousInit() {
   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 (autonomousCommand != null) {
-      Scheduler.getDefault().cancel(autonomousCommand);
-    }
-  }
+  public void teleopInit() {}
 
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {}
 
   @Override
   public void utilityInit() {
-    // Cancels all running commands at the start of utility mode.
+    // Cancel all running commands at the start of utility mode.
     Scheduler.getDefault().cancelAll();
   }
 

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/commands/Autos.java

@@ -6,81 +6,36 @@
 
 import org.wpilib.command3.Command;
 import org.wpilib.examples.hatchbotcmdv3.Constants.AutoConstants;
-import org.wpilib.examples.hatchbotcmdv3.subsystems.DriveSubsystem;
-import org.wpilib.examples.hatchbotcmdv3.subsystems.HatchSubsystem;
+import org.wpilib.examples.hatchbotcmdv3.mechanisms.DriveMechanism;
+import org.wpilib.examples.hatchbotcmdv3.mechanisms.HatchMechanism;
 
 /** 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 drive
-        .run(
-            coro -> {
-              drive.resetEncoders();
-              while (drive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches) {
-                drive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0);
-                coro.yield();
-              }
-              drive.arcadeDrive(0, 0);
-            })
-        .whenCanceled(
-            () -> {
-              drive.arcadeDrive(0, 0);
-            })
-        .named("Simple Auto");
+  /** A simple auto routine that drives forward a specified distance and then stops. */
+  public static Command simpleAuto(DriveMechanism drive) {
+    return drive.driveDistance(
+        AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed);
   }
 
   /** A complex auto routine that drives forward, drops a hatch, and then drives backward. */
-  public static Command complexAuto(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
+  public static Command complexAuto(DriveMechanism driveMechanism, HatchMechanism hatchMechanism) {
     // NOTE: requirement behavior.
     // To require each mechanism for while it's active, replace `requiring` with `noRequirements`.
-    return Command.requiring(driveSubsystem, hatchSubsystem)
+    return Command.requiring(driveMechanism, hatchMechanism)
         .executing(
             coro -> {
               // Drive forward up to the front of the cargo ship
               coro.await(
-                  driveSubsystem
-                      .run(
-                          coro2 -> {
-                            // Reset encoders on command start
-                            driveSubsystem.resetEncoders();
-                            // End the command when the robot's driven distance exceeds the desired
-                            // value
-                            while (driveSubsystem.getAverageEncoderDistance()
-                                >= AutoConstants.kAutoDriveDistanceInches) {
-                              // Drive forward while the command is executing
-                              driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0);
-                              coro2.yield();
-                            }
-                            // Stop driving at the end of the command
-                            driveSubsystem.arcadeDrive(0, 0);
-                          })
-                      .whenCanceled(() -> driveSubsystem.arcadeDrive(0, 0))
-                      .named("Part 1"));
+                  driveMechanism.driveDistance(
+                      AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed));
 
               // Release the hatch
-              coro.await(hatchSubsystem.releaseHatchCommand());
+              coro.await(hatchMechanism.releaseHatchCommand());
 
               // Drive backward the specified distance
               coro.await(
-                  driveSubsystem
-                      .run(
-                          coro2 -> {
-                            // Reset encoders on command start
-                            driveSubsystem.resetEncoders();
-                            // End the command when the robot's driven distance exceeds the desired
-                            // value
-                            while (driveSubsystem.getAverageEncoderDistance()
-                                >= AutoConstants.kAutoDriveDistanceInches) {
-                              // Drive backward while the command is executing
-                              driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0);
-                              coro2.yield();
-                            }
-                            // Stop driving at the end of the command
-                            driveSubsystem.arcadeDrive(0, 0);
-                          })
-                      .whenCanceled(() -> driveSubsystem.arcadeDrive(0, 0))
-                      .named("Part 3"));
+                  driveMechanism.driveDistance(
+                      AutoConstants.kAutoBackupDistanceInches, AutoConstants.kAutoDriveSpeed));
             })
         .named("Complex Auto");
   }