diff --git a/wpilibjExamples/build_java_examples.bzl b/wpilibjExamples/build_java_examples.bzl
index 2246e93966..af82380f2d 100644
--- a/wpilibjExamples/build_java_examples.bzl
+++ b/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",
diff --git a/wpilibjExamples/example_projects.bzl b/wpilibjExamples/example_projects.bzl
index 748909113f..9a50696f18 100644
--- a/wpilibjExamples/example_projects.bzl
+++ b/wpilibjExamples/example_projects.bzl
@@ -13,6 +13,7 @@ EXAMPLE_FOLDERS = [
"encoder",
"gettingstarted",
"gyro",
+ "hatchbotcmdv3",
"hatchbotinlined",
"hatchbottraditional",
"mecanumbot",
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json
index 52567ec59e..12ddf341c5 100644
--- a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json
+++ b/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.",
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java
new file mode 100644
index 0000000000..8b0228d35f
--- /dev/null
+++ b/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.
+ *
+ *
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;
+ }
+}
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java
new file mode 100644
index 0000000000..8e3b9d99fe
--- /dev/null
+++ b/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.
+ *
+ *
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() {}
+}
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java
new file mode 100644
index 0000000000..de77349661
--- /dev/null
+++ b/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 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();
+ }
+}
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/commands/Autos.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/commands/Autos.java
new file mode 100644
index 0000000000..c816f23c05
--- /dev/null
+++ b/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!");
+ }
+}
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java
new file mode 100644
index 0000000000..d8056e96d5
--- /dev/null
+++ b/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);
+ // }
+}
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java
new file mode 100644
index 0000000000..3790748d22
--- /dev/null
+++ b/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);
+ // }
+}