Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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>
This commit is contained in:
sciencewhiz
2026-04-25 15:06:26 -07:00
committed by GitHub
parent 35e8abedeb
commit 6aa1611959
9 changed files with 500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
wpilibjExamples/build_java_examples.bzl
View File

@@ -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",

1
wpilibjExamples/example_projects.bzl
View File

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

18
wpilibjExamples/src/main/java/org/wpilib/examples/examples.json
View File

@@ -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.",

48
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java Normal file
View File

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

102
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java Normal file
View File

@@ -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() {}
}

96
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java Normal file
View File

@@ -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();
}
}

91
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/commands/Autos.java Normal file
View File

@@ -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!");
}
}

100
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java Normal file
View File

@@ -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);
// }
}

43
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java Normal file
View File

@@ -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);
// }
}