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