[examples] Add a Hatchbot Commands v3 example (#8809)

This is adapted from the work that @Starlight220 did here:
https://github.com/SamCarlberg/allwpilib/pull/5

---------

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

wpilibjExamples/build_java_examples.bzl

@@ -47,6 +47,7 @@ def build_examples(halsim_deps):
                 "//wpimath:wpimath-java",
                 "//wpilibj:wpilibj-java",
                 "//commandsv2:commandsv2-java",
+                "//commandsv3:commandsv3-java",
                 "//wpiutil:wpiutil-java",
                 "//romiVendordep:romiVendordep-java",
                 "//xrpVendordep:xrpVendordep-java",

wpilibjExamples/example_projects.bzl

@@ -13,6 +13,7 @@ EXAMPLE_FOLDERS = [
     "encoder",
     "gettingstarted",
     "gyro",
+    "hatchbotcmdv3",
     "hatchbotinlined",
     "hatchbottraditional",
     "mecanumbot",

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

@@ -149,6 +149,24 @@
     "robotclass": "Robot",
     "commandversion": 2
   },
+  {
+    "name": "Hatchbot Commandsv3",
+    "description": "A fully-functional Commandv3 hatchbot for the 2019 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",
+    "tags": [
+      "Complete Robot",
+      "Commandv3",
+      "Differential Drive",
+      "Encoder",
+      "Pneumatics",
+      "Sendable",
+      "DataLog",
+      "Gamepad"
+    ],
+    "foldername": "hatchbotcmdv3",
+    "gradlebase": "java",
+    "robotclass": "Robot",
+    "commandversion": 3
+  },
   {
     "name": "Rapid React Command Bot",
     "description": "A fully-functional Commandv2 fender bot for the 2022 game, written in the 'inlined' style, i.e. many commands are defined inline with lambdas.",

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

@@ -0,0 +1,48 @@
+// 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 org.wpilib.examples.hatchbotcmdv3;
+
+/**
+ * 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 = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
+    public static final boolean kLeftEncoderReversed = false;
+    public static final boolean kRightEncoderReversed = true;
+
+    public static final int kEncoderCPR = 1024;
+    public static final double kWheelDiameterInches = 6;
+    public static final double kEncoderDistancePerPulse =
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterInches * Math.PI) / kEncoderCPR;
+  }
+
+  public static final class HatchConstants {
+    public static final int kHatchSolenoidModule = 0;
+    public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
+  }
+
+  public static final class AutoConstants {
+    public static final double kAutoDriveDistanceInches = 60;
+    public static final double kAutoBackupDistanceInches = 20;
+    public static final double kAutoDriveSpeed = 0.5;
+  }
+
+  public static final class OIConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+}

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

@@ -0,0 +1,102 @@
+// 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 org.wpilib.examples.hatchbotcmdv3;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.Scheduler;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.framework.TimedRobot;
+import org.wpilib.system.DataLogManager;
+
+/**
+ * The methods in this class are called automatically 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 Main.java file in the project.
+ */
+public class Robot extends TimedRobot {
+  private Command autonomousCommand;
+
+  private final RobotContainer robotContainer;
+
+  /**
+   * 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();
+
+    // Start recording to data log
+    DataLogManager.start();
+
+    // Record DS control and joystick data.
+    // Change to `false` to not record joystick data.
+    DriverStation.startDataLog(DataLogManager.getLog(), true);
+  }
+
+  /**
+   * 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.
+   *
+   * <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.
+    Scheduler.getDefault().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    autonomousCommand = robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (autonomousCommand != null) {
+      Scheduler.getDefault().schedule(autonomousCommand);
+    }
+  }
+
+  /** 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 (autonomousCommand != null) {
+      Scheduler.getDefault().cancel(autonomousCommand);
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void utilityInit() {
+    // Cancels all running commands at the start of test mode.
+    Scheduler.getDefault().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void utilityPeriodic() {}
+}

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

@@ -0,0 +1,96 @@
+// 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 org.wpilib.examples.hatchbotcmdv3;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.button.CommandGamepad;
+import org.wpilib.driverstation.Gamepad;
+import org.wpilib.examples.hatchbotcmdv3.Constants.OIConstants;
+import org.wpilib.examples.hatchbotcmdv3.commands.Autos;
+import org.wpilib.examples.hatchbotcmdv3.subsystems.DriveSubsystem;
+import org.wpilib.examples.hatchbotcmdv3.subsystems.HatchSubsystem;
+import org.wpilib.smartdashboard.SendableChooser;
+import org.wpilib.smartdashboard.SmartDashboard;
+
+/**
+ * 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 RobotContainer {
+  // The robot's subsystems
+  private final DriveSubsystem robotDrive = new DriveSubsystem();
+  private final HatchSubsystem 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 simpleAuto = Autos.simpleAuto(robotDrive);
+  // A complex auto routine that drives forward, drops a hatch, and then drives backward.
+  private final Command complexAuto = Autos.complexAuto(robotDrive, hatchSubsystem);
+
+  // A chooser for autonomous commands
+  SendableChooser<Command> chooser = new SendableChooser<>();
+
+  // The driver's controller
+  CommandGamepad driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+
+    // Configure default commands
+    // Set the default drive command to split-stick arcade drive
+    robotDrive.setDefaultCommand(
+        // A split-stick arcade command, with forward/backward controlled by the left
+        // hand, and turning controlled by the right.
+        robotDrive
+            .runRepeatedly(
+                () ->
+                    robotDrive.arcadeDrive(
+                        -driverController.getLeftY(), -driverController.getRightX()))
+            .withPriority(Command.LOWEST_PRIORITY)
+            .named("Split-Stick Arcade Drive (Default Command)"));
+
+    // Add commands to the autonomous command chooser
+    chooser.setDefaultOption("Simple Auto", simpleAuto);
+    chooser.addOption("Complex Auto", complexAuto);
+
+    // Put the chooser on the dashboard
+    SmartDashboard.putData("Autonomous", chooser);
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link org.wpilib.driverstation.GenericHID} or one of its subclasses ({@link
+   * org.wpilib.driverstation.Joystick} or {@link Gamepad}), and then passing it to a {@link
+   * org.wpilib.command3.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Grab the hatch when the Circle button is pressed.
+    driverController.eastFace().onTrue(hatchSubsystem.grabHatchCommand());
+    // Release the hatch when the Square button is pressed.
+    driverController.westFace().onTrue(hatchSubsystem.releaseHatchCommand());
+    // While holding R1, drive at half speed
+    driverController
+        .rightBumper()
+        .onTrue(
+            Command.noRequirements(coro -> robotDrive.setMaxOutput(0.5)).named("Set half speed"))
+        .onFalse(
+            Command.noRequirements(coro -> robotDrive.setMaxOutput(1)).named("Set full speed"));
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return chooser.getSelected();
+  }
+}

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

@@ -0,0 +1,91 @@
+// 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 org.wpilib.examples.hatchbotcmdv3.commands;
+
+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;
+
+/** 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 complex auto routine that drives forward, drops a hatch, and then drives backward. */
+  public static Command complexAuto(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
+    // NOTE: requirement behavior.
+    // To require each mechanism for while it's active, replace `requiring` with `noRequirements`.
+    return Command.requiring(driveSubsystem, hatchSubsystem)
+        .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"));
+
+              // Release the hatch
+              coro.await(hatchSubsystem.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"));
+            })
+        .named("Complex Auto");
+  }
+
+  private Autos() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java

@@ -0,0 +1,100 @@
+// 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 org.wpilib.examples.hatchbotcmdv3.subsystems;
+
+import org.wpilib.command3.Mechanism;
+import org.wpilib.drive.DifferentialDrive;
+import org.wpilib.examples.hatchbotcmdv3.Constants.DriveConstants;
+import org.wpilib.hardware.motor.PWMSparkMax;
+import org.wpilib.hardware.rotation.Encoder;
+import org.wpilib.util.sendable.SendableRegistry;
+
+public class DriveSubsystem extends Mechanism {
+  // The motors on the left side of the drive.
+  private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
+  private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
+
+  // The motors on the right side of the drive.
+  private final PWMSparkMax rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
+  private final PWMSparkMax rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
+
+  // The robot's drive
+  private final DifferentialDrive drive =
+      new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
+
+  // The left-side drive encoder
+  private final Encoder leftEncoder =
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
+
+  // The right-side drive encoder
+  private final Encoder rightEncoder =
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
+
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {
+    SendableRegistry.addChild(drive, leftLeader);
+    SendableRegistry.addChild(drive, rightLeader);
+
+    leftLeader.addFollower(leftFollower);
+    rightLeader.addFollower(rightFollower);
+
+    // 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.
+    rightLeader.setInverted(true);
+
+    // Sets the distance per pulse for the encoders
+    leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+    rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+  }
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param fwd the commanded forward movement
+   * @param rot the commanded rotation
+   */
+  public void arcadeDrive(double fwd, double rot) {
+    drive.arcadeDrive(fwd, rot);
+  }
+
+  /** Resets the drive encoders to currently read a position of 0. */
+  public void resetEncoders() {
+    leftEncoder.reset();
+    rightEncoder.reset();
+  }
+
+  /**
+   * Gets the average distance of the TWO encoders.
+   *
+   * @return the average of the TWO encoder readings
+   */
+  public double getAverageEncoderDistance() {
+    return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2.0;
+  }
+
+  /**
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
+   *
+   * @param maxOutput the maximum output to which the drive will be constrained
+   */
+  public void setMaxOutput(double maxOutput) {
+    drive.setMaxOutput(maxOutput);
+  }
+
+  //  @Override
+  //  public void initSendable(SendableBuilder builder) {
+  //    super.initSendable(builder);
+  //    // Publish encoder distances to telemetry.
+  //    builder.addDoubleProperty("leftDistance", leftEncoder::getDistance, null);
+  //    builder.addDoubleProperty("rightDistance", rightEncoder::getDistance, null);
+  //  }
+}

wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java

@@ -0,0 +1,43 @@
+// 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 org.wpilib.examples.hatchbotcmdv3.subsystems;
+
+import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.FORWARD;
+import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.REVERSE;
+
+import org.wpilib.command3.Command;
+import org.wpilib.command3.Mechanism;
+import org.wpilib.examples.hatchbotcmdv3.Constants.HatchConstants;
+import org.wpilib.hardware.pneumatic.DoubleSolenoid;
+import org.wpilib.hardware.pneumatic.PneumaticsModuleType;
+
+/** A hatch mechanism actuated by a single {@link org.wpilib.hardware.pneumatic.DoubleSolenoid}. */
+public class HatchSubsystem extends Mechanism {
+  private final DoubleSolenoid hatchSolenoid =
+      new DoubleSolenoid(
+          0,
+          PneumaticsModuleType.CTRE_PCM,
+          HatchConstants.kHatchSolenoidPorts[0],
+          HatchConstants.kHatchSolenoidPorts[1]);
+
+  /** Grabs the hatch. */
+  public Command grabHatchCommand() {
+    // implicitly require `this`
+    return this.run(coro -> hatchSolenoid.set(FORWARD)).named("Grab Hatch");
+  }
+
+  /** Releases the hatch. */
+  public Command releaseHatchCommand() {
+    // implicitly require `this`
+    return this.run(coro -> hatchSolenoid.set(REVERSE)).named("Release Hatch");
+  }
+
+  //  @Override
+  //  public void initSendable(SendableBuilder builder) {
+  //    super.initSendable(builder);
+  //    // Publish the solenoid state to telemetry.
+  //    builder.addBooleanProperty("extended", () -> hatchSolenoid.get() == kForward, null);
+  //  }
+}