diff --git a/wpilibjExamples/build_java_examples.bzl b/wpilibjExamples/build_java_examples.bzl index af82380f2db..aff6b65a6b0 100644 --- a/wpilibjExamples/build_java_examples.bzl +++ b/wpilibjExamples/build_java_examples.bzl @@ -81,6 +81,9 @@ def build_snippets(): java_library( name = folder + "-snippet", srcs = native.glob(["src/main/java/org/wpilib/snippets/" + folder + "/**/*.java"]), + plugins = [ + "//epilogue-processor:plugin", + ], deps = [ "//apriltag:apriltag-java", "//cameraserver:cameraserver-java", @@ -90,6 +93,7 @@ def build_snippets(): "//wpimath:wpimath-java", "//wpilibj:wpilibj-java", "//commandsv2:commandsv2-java", + "//commandsv3:commandsv3-java", "//wpiutil:wpiutil-java", "//romiVendordep:romiVendordep-java", "//xrpVendordep:xrpVendordep-java", @@ -107,13 +111,19 @@ def build_templates(): java_library( name = folder + "-template", srcs = native.glob(["src/main/java/org/wpilib/templates/" + folder + "/**/*.java"]), + plugins = [ + "//epilogue-processor:plugin", + ], deps = [ "//hal:hal-java", "//wpilibj:wpilibj-java", "//commandsv2:commandsv2-java", + "//commandsv3:commandsv3-java", "//wpimath:wpimath-java", "//wpiutil:wpiutil-java", + "//epilogue-runtime:epilogue-java", "//xrpVendordep:xrpVendordep-java", + "//wpiunits:wpiunits-java", ], tags = ["wpi-example"], ) @@ -123,14 +133,20 @@ def build_tests(): wpilib_java_junit5_test( name = folder + "-test", srcs = native.glob(["src/test/java/org/wpilib/examples/" + folder + "/**/*.java"]), + plugins = [ + "//epilogue-processor:plugin", + ], deps = [ ":" + folder + "-example", "//hal:hal-java", "//ntcore:ntcore-java", "//wpilibj:wpilibj-java", "//commandsv2:commandsv2-java", + "//commandsv3:commandsv3-java", "//wpimath:wpimath-java", "//wpiutil:wpiutil-java", + "//epilogue-runtime:epilogue-java", + "//wpiunits:wpiunits-java", ], tags = ["wpi-example"], ) @@ -139,14 +155,20 @@ def build_tests(): wpilib_java_junit5_test( name = folder + "-test", srcs = native.glob(["src/test/java/org/wpilib/snippets/" + folder + "/**/*.java"]), + plugins = [ + "//epilogue-processor:plugin", + ], deps = [ ":" + folder + "-snippet", "//hal:hal-java", "//ntcore:ntcore-java", "//wpilibj:wpilibj-java", "//commandsv2:commandsv2-java", + "//commandsv3:commandsv3-java", "//wpimath:wpimath-java", "//wpiutil:wpiutil-java", + "//epilogue-runtime:epilogue-java", + "//wpiunits:wpiunits-java", ], tags = ["wpi-example"], ) diff --git a/wpilibjExamples/example_projects.bzl b/wpilibjExamples/example_projects.bzl index d3b78e18b7d..135b7020ebb 100644 --- a/wpilibjExamples/example_projects.bzl +++ b/wpilibjExamples/example_projects.bzl @@ -22,6 +22,7 @@ EXAMPLE_FOLDERS = [ "mecanumdriveposeestimator", "mechanism2d", "rapidreactcommandbot", + "rebuiltcmdv3", "romireference", "simpledifferentialdrivesimulation", "statespacearm", @@ -85,6 +86,8 @@ SNIPPET_FOLDERS = [ TEMPLATE_FOLDERS = [ "commandv2", "commandv2skeleton", + "commandv3", + "commandv3skeleton", "educational", "opmode", "robotbaseskeleton", diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json index 4d7839cfde1..89dc277fe83 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json +++ b/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.", diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java index 8b0228d35f9..7f13b0033e9 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Constants.java +++ b/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; } diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java index 23cf510c16b..0ac50b29b34 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/Robot.java +++ b/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 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,15 +120,7 @@ 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 @@ -92,7 +128,7 @@ 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(); } diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java deleted file mode 100644 index 48f23ac9efd..00000000000 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java +++ /dev/null @@ -1,96 +0,0 @@ -// 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.faceRight().onTrue(hatchSubsystem.grabHatchCommand()); - // Release the hatch when the Square button is pressed. - driverController.faceLeft().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 index c816f23c05b..a6484f19077 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/commands/Autos.java +++ b/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"); } diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/DriveMechanism.java similarity index 64% rename from wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/DriveMechanism.java index 71075e06f9b..08db58b0286 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/DriveMechanism.java @@ -2,8 +2,10 @@ // 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; +package org.wpilib.examples.hatchbotcmdv3.mechanisms; +import java.util.function.DoubleSupplier; +import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; import org.wpilib.drive.DifferentialDrive; import org.wpilib.examples.hatchbotcmdv3.Constants.DriveConstants; @@ -11,7 +13,7 @@ import org.wpilib.hardware.rotation.Encoder; import org.wpilib.util.sendable.SendableRegistry; -public class DriveSubsystem implements Mechanism { +public class DriveMechanism implements 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); @@ -38,8 +40,8 @@ public class DriveSubsystem implements Mechanism { DriveConstants.kRightEncoderPorts[1], DriveConstants.kRightEncoderReversed); - /** Creates a new DriveSubsystem. */ - public DriveSubsystem() { + /** Creates a new DriveMechanism. */ + public DriveMechanism() { SendableRegistry.addChild(drive, leftLeader); SendableRegistry.addChild(drive, rightLeader); @@ -59,15 +61,50 @@ public DriveSubsystem() { /** * Drives the robot using arcade controls. * - * @param fwd the commanded forward movement - * @param rot the commanded rotation + * @param speed The speed to drive at, from -1 (full reverse) to +1 (full forward) + * @param turn The turn rate, from -1 (full counterclockwise) to +1 (full clockwise) + * @return A command that drives the robot using arcade controls */ - public void arcadeDrive(double fwd, double rot) { - drive.arcadeDrive(fwd, rot); + public Command arcadeDrive(DoubleSupplier speed, DoubleSupplier turn) { + return runRepeatedly(() -> drive.arcadeDrive(speed.getAsDouble(), turn.getAsDouble())) + .named("Arcade Drive"); + } + + /** + * Drives a set distance, in inches, at a set speed ratio from 0-1. The command will exit when the + * distance has been reached. + * + * @param distance How many inches to travel. This may be negative to drive backwards. + * @param speed Speed ratio from 0 (off) to 1 (full speed). Negative values will make the robot + * drive the wrong way. + * @return A command that drives a specified distance. + */ + public Command driveDistance(double distance, double speed) { + return run(coroutine -> { + resetEncoders(); + + if (distance >= 0) { + // drive forward + while (getAverageEncoderDistance() < distance) { + drive.tankDrive(speed, 0); + coroutine.yield(); + } + } else { + // drive backward + while (getAverageEncoderDistance() > distance) { + drive.tankDrive(-speed, 0); + coroutine.yield(); + } + } + + // Finally, stop + drive.stopMotor(); + }) + .named("Drive Distance[" + distance + "@" + speed + "]"); } /** Resets the drive encoders to currently read a position of 0. */ - public void resetEncoders() { + private void resetEncoders() { leftEncoder.reset(); rightEncoder.reset(); } diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/HatchMechanism.java similarity index 93% rename from wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/HatchMechanism.java index 14116b6e56a..9bbbf54e007 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/subsystems/HatchSubsystem.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/mechanisms/HatchMechanism.java @@ -2,7 +2,7 @@ // 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; +package org.wpilib.examples.hatchbotcmdv3.mechanisms; import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.FORWARD; import static org.wpilib.hardware.pneumatic.DoubleSolenoid.Value.REVERSE; @@ -14,7 +14,7 @@ import org.wpilib.hardware.pneumatic.PneumaticsModuleType; /** A hatch mechanism actuated by a single {@link org.wpilib.hardware.pneumatic.DoubleSolenoid}. */ -public class HatchSubsystem implements Mechanism { +public class HatchMechanism implements Mechanism { private final DoubleSolenoid hatchSolenoid = new DoubleSolenoid( 0, diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java new file mode 100644 index 00000000000..5bebb934895 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java @@ -0,0 +1,67 @@ +// 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.rebuiltcmdv3; + +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.DriveConstants; +import org.wpilib.math.estimator.SwerveDrivePoseEstimator; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rectangle2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; + +@Logged +public class PoseEstimator { + private final SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator( + DriveConstants.KINEMATICS, + Rotation2d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + Pose2d.kZero); + + /** + * Updates the pose estimator with the current gyro heading and swerve module positions. + * + * @param gyroHeading The current gyro heading + * @param modulePositions The current module positions + */ + public void odometryUpdate(Rotation2d gyroHeading, SwerveModulePosition... modulePositions) { + poseEstimator.update(gyroHeading, modulePositions); + } + + /** + * Updates the pose estimator with a vision measurement. + * + * @param estimatedPose The estimated pose from vision + * @param timestamp The timestamp of the vision measurement + */ + public void visionUpdate(Pose2d estimatedPose, double timestamp) { + poseEstimator.addVisionMeasurement(estimatedPose, timestamp); + } + + /** + * Returns the current estimated pose of the robot. + * + * @return The current estimated pose + */ + public Pose2d getEstimatedPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** + * Checks if the current estimated pose is within a given rectangular zone. + * + * @param bounds The rectangular zone to check against + * @return True if the pose is within the zone, false otherwise + */ + public boolean inZone(Rectangle2d bounds) { + return bounds.contains(poseEstimator.getEstimatedPosition().getTranslation()); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java new file mode 100644 index 00000000000..9950adb7b5c --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java @@ -0,0 +1,75 @@ +// 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.rebuiltcmdv3; + +import static org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants.NEUTRAL_ZONE; +import static org.wpilib.units.Units.Meters; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Scheduler; +import org.wpilib.command3.Trigger; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.epilogue.Epilogue; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.mechanisms.Intake; +import org.wpilib.examples.rebuiltcmdv3.mechanisms.Shooter; +import org.wpilib.examples.rebuiltcmdv3.mechanisms.SwerveDrive; +import org.wpilib.framework.OpModeRobot; +import org.wpilib.math.geometry.Pose2d; + +@Logged +public class Robot extends OpModeRobot { + public final SwerveDrive swerveDrive = new SwerveDrive(); + public final Shooter shooter = new Shooter(); + public final Intake intake = new Intake(); + public final PoseEstimator poseEstimator = new PoseEstimator(); + public final CommandGamepad controller = new CommandGamepad(1); + + public final Trigger inNeutralZone = new Trigger(() -> poseEstimator.inZone(NEUTRAL_ZONE)); + + /** Initializes the robot class and sets safe default commands for all its mechanisms. */ + public Robot() { + swerveDrive.setDefaultCommand(swerveDrive.idle()); + shooter.setDefaultCommand(shooter.idle()); + intake.setDefaultCommand(intake.idle()); + } + + @Override + public void robotPeriodic() { + // 1. Update odometry so commands will be working with up-to-date information. + poseEstimator.odometryUpdate(swerveDrive.getGyroHeading(), swerveDrive.getModulePositions()); + + // 2. Run the scheduler to poll triggers and execute our commands. + Scheduler.getDefault().run(); + + // 3. Update telemetry. + Epilogue.update(this); + } + + /** + * A command that uses all the mechanisms on the robot to aim and shoot at a specific target. + * + * @param targetPose The target position to shoot at. + * @return A command to shoot at a target + */ + public Command shootAt(Pose2d targetPose) { + return Command.noRequirements( + coroutine -> { + coroutine.await(swerveDrive.aimAt(poseEstimator::getEstimatedPose, targetPose)); + + coroutine.awaitAll( + intake.agitate(), + shooter.shootAtHub( + () -> { + return Meters.of( + targetPose + .minus(poseEstimator.getEstimatedPose()) + .getTranslation() + .getNorm()); + })); + }) + .named("Robot.ShootAt[" + targetPose + "]"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.java new file mode 100644 index 00000000000..99db87f2666 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.java @@ -0,0 +1,28 @@ +// 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.rebuiltcmdv3.constants; + +import static org.wpilib.units.Units.FeetPerSecond; +import static org.wpilib.units.Units.RotationsPerSecond; + +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.LinearVelocity; + +public final class DriveConstants { + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(); + public static final LinearVelocity MAX_VELOCITY = FeetPerSecond.of(15); + public static final AngularVelocity MAX_TURN_RATE = RotationsPerSecond.of(3); + + // All drive IDs are odd, all turn IDs are even + public static final int FRONT_LEFT_DRIVE_ID = 1; + public static final int FRONT_LEFT_TURN_ID = 2; + public static final int FRONT_RIGHT_DRIVE_ID = 3; + public static final int FRONT_RIGHT_TURN_ID = 4; + public static final int REAR_LEFT_DRIVE_ID = 5; + public static final int REAR_LEFT_TURN_ID = 6; + public static final int REAR_RIGHT_DRIVE_ID = 7; + public static final int REAR_RIGHT_TURN_ID = 8; +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.java new file mode 100644 index 00000000000..09e01d9a5fe --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.java @@ -0,0 +1,39 @@ +// 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.rebuiltcmdv3.constants; + +import java.util.Optional; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rectangle2d; +import org.wpilib.math.geometry.Translation2d; + +public final class FieldConstants { + // TODO: Measurements + public static final Pose2d RED_HUB_CENTER = new Pose2d(); + public static final Pose2d BLUE_HUB_CENTER = new Pose2d(); + public static final Rectangle2d NEUTRAL_ZONE = + new Rectangle2d(new Translation2d(), new Translation2d()); + public static final Rectangle2d RED_ZONE = + new Rectangle2d(new Translation2d(), new Translation2d()); + public static final Rectangle2d BLUE_ZONE = + new Rectangle2d(new Translation2d(), new Translation2d()); + + /** + * Gets location of our hub. Returns an empty optional if we don't have an alliance yet. + * + * @return The location of our hub. + */ + public static Optional targetHub() { + return DriverStationBackend.getAlliance() + .map( + a -> { + return switch (a) { + case RED -> RED_HUB_CENTER; + case BLUE -> BLUE_HUB_CENTER; + }; + }); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.java new file mode 100644 index 00000000000..f333d3f892b --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.java @@ -0,0 +1,25 @@ +// 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.rebuiltcmdv3.constants; + +import static org.wpilib.units.Units.Degrees; +import static org.wpilib.units.Units.RPM; + +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; + +public final class IntakeConstants { + public static final int ROLLER_MOTOR_ID = 20; + public static final int WRIST_MOTOR_ID = 21; + + public static final Angle WRIST_STOW_ANGLE = Degrees.of(90); + public static final Angle WRIST_DOWN_ANGLE = Degrees.of(-10); + + public static final Angle WRIST_AGITATE_UP = Degrees.of(10); + public static final Angle WRIST_AGITATE_DOWN = WRIST_DOWN_ANGLE; + + public static final AngularVelocity ROLLER_INTAKE_SPEED = RPM.of(4000); + public static final AngularVelocity ROLLER_EXPULSION_SPEED = RPM.of(-3500); +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.java new file mode 100644 index 00000000000..6b2c3a80c1a --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.java @@ -0,0 +1,23 @@ +// 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.rebuiltcmdv3.constants; + +public final class ShooterConstants { + public static final int HOOD_MOTOR_ID = 10; + public static final int PRIMARY_SHOOTER_MOTOR_ID = 11; + public static final int SECONDARY_SHOOTER_MOTOR_ID = 12; + public static final int TERTIARY_SHOOTER_MOTOR_ID = 13; + public static final int QUATERNARY_SHOOTER_MOTOR_ID = 14; + + public static final double HOOD_KP = 80; + + /** Example motor velocity feedforward constant, in volts per RPM. */ + public static final double EXAMPLE_MOTOR_KV = 12d / 6000d; + + /** Voltage necessary to break static friction to get the flywheel moving. */ + public static final double FLYWHEEL_KS = 1.25; + + public static final double FLYWHEEL_KP = 100; +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.java new file mode 100644 index 00000000000..3595e406f6f --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.java @@ -0,0 +1,57 @@ +// 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.rebuiltcmdv3.lookup; + +import static org.wpilib.units.Units.Value; + +import org.wpilib.math.interpolation.Interpolator; +import org.wpilib.math.interpolation.InverseInterpolator; +import org.wpilib.units.Measure; +import org.wpilib.units.Unit; +import org.wpilib.units.measure.Dimensionless; + +/** Utility class for working with unit-based interpolating tree maps. */ +public final class LookupTables { + private LookupTables() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Creates an interpolator operating on Measure objects. + * + * @param The type of the unit for the measure + * @param The type of the measure + * @return The interpolator for the measure + */ + @SuppressWarnings("unchecked") + public static > Interpolator unitInterpolator() { + return (startValue, endValue, t) -> + (M) endValue.minus(startValue).times(Math.clamp(t, 0, 1)).plus(startValue); + } + + /** + * Creates an inverse interpolator operating on Measure objects. + * + * @param The type of the unit for the measure + * @param The type of the measure + * @return The inverse interpolator for the measure + */ + public static > + InverseInterpolator inverseUnitInterpolator() { + return (startValue, endValue, q) -> { + Measure totalRange = endValue.minus(startValue); + if (totalRange.baseUnitMagnitude() <= 0) { + return 0; + } + + Measure queryToStart = q.minus(startValue); + if (queryToStart.baseUnitMagnitude() <= 0) { + return 0; + } + + return ((Dimensionless) queryToStart.div(totalRange)).in(Value); + }; + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java new file mode 100644 index 00000000000..2307f6d2b62 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java @@ -0,0 +1,92 @@ +// 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.rebuiltcmdv3.mechanisms; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; + +@Logged +public class Intake implements Mechanism { + private final IntakeWrist wrist = new IntakeWrist(); + private final IntakeRoller roller = new IntakeRoller(); + + /** + * Intakes fuel off the ground. This command will run until canceled or interrupted by another + * command. + * + * @return An intake command + */ + public Command intake() { + return run(coroutine -> { + coroutine.awaitAll(wrist.down(), roller.intake()); + }) + .named("Intake.Intake"); + } + + /** + * Stops the intake mechanism. This stops the roller and depowers the wrist, which may sag under + * gravity. + * + * @return A command that stops the intake mechanism + */ + public Command stop() { + return run(coroutine -> { + coroutine.awaitAll(wrist.stop(), roller.stop()); + }) + .withPriority(Command.LOWEST_PRIORITY) + .named("Intake.Idle"); + } + + @Override + public Command idle() { + return stop(); + } + + /** + * Stows the intake mechanism. This raises the wrist and stops the roller. + * + * @return A command that stows the intake mechanism + */ + public Command stow() { + return run(coroutine -> { + coroutine.awaitAll(wrist.stow(), roller.stop()); + }) + .named("Intake.Stow"); + } + + /** + * Uses the intake mechanism to agitate fuel inside the hopper. The roller will spin to encourage + * fuel to move around and help unblock jams. + * + * @return A command that agitates the intake mechanism + */ + public Command agitate() { + return run(coroutine -> { + coroutine.fork(roller.intake()); + + while (true) { + // await() will yield internally until the wrist has moved to the desired position, + // so we don't need to explicitly yield here + coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_UP)); + coroutine.await(wrist.moveToAngle(IntakeConstants.WRIST_AGITATE_DOWN)); + } + }) + .named("Intake.Agitate"); + } + + /** + * Uses the intake mechanism to expel fuel from the hopper. + * + * @return A command that expels fuel from the intake mechanism + */ + public Command outtake() { + return run(coroutine -> { + coroutine.awaitAll(wrist.down(), roller.expel()); + }) + .named("Intake.Outtake"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java new file mode 100644 index 00000000000..2e0514543b2 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java @@ -0,0 +1,67 @@ +// 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.rebuiltcmdv3.mechanisms; + +import static org.wpilib.units.Units.RadiansPerSecond; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; + +@Logged +public class IntakeRoller implements Mechanism { + private final ExampleSmartMotorController motor = + new ExampleSmartMotorController(IntakeConstants.ROLLER_MOTOR_ID); + + /** + * Stops the roller motor. This command keeps the roller off until interrupted by another command. + * + * @return Command to stop the roller + */ + public Command stop() { + return runRepeatedly(motor::stopMotor).named("Intake.Roller.Stop"); + } + + @Override + public Command idle() { + return stop(); + } + + /** + * Starts the roller motor in intake mode. This command keeps the roller running until interrupted + * by another command. + * + * @return Command to start the roller in intake mode + */ + public Command intake() { + return run(coroutine -> { + motor.setSetpoint( + ExampleSmartMotorController.PIDMode.kVelocity, + IntakeConstants.ROLLER_INTAKE_SPEED.in(RadiansPerSecond)); + + coroutine.park(); + }) + .named("Intake.Roller.Intake"); + } + + /** + * Starts the roller motor in expel mode. This command keeps the roller running until interrupted + * by another command. + * + * @return Command to start the roller in expel mode + */ + public Command expel() { + return run(coroutine -> { + motor.setSetpoint( + ExampleSmartMotorController.PIDMode.kVelocity, + IntakeConstants.ROLLER_EXPULSION_SPEED.in(RadiansPerSecond)); + + coroutine.park(); + }) + .named("Intake.Roller.Expel"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.java new file mode 100644 index 00000000000..5a58b702a64 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.java @@ -0,0 +1,83 @@ +// 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.rebuiltcmdv3.mechanisms; + +import static org.wpilib.units.Units.Degrees; +import static org.wpilib.units.Units.Radians; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; +import org.wpilib.units.measure.Angle; + +@Logged +public class IntakeWrist implements Mechanism { + private final ExampleSmartMotorController motor = + new ExampleSmartMotorController(IntakeConstants.WRIST_MOTOR_ID); + + public Command stop() { + return runRepeatedly(motor::stopMotor).named("Intake.Wrist.Stop"); + } + + @Override + public Command idle() { + return stop(); + } + + /** + * Moves the wrist to a specific angle. The command will exit when the target angle is reached. + * + * @param target The target angle to move to + * @return A command that moves the wrist to the target angle + */ + public Command moveToAngle(Angle target) { + double targetRads = target.in(Radians); + double tolerance = Radians.convertFrom(0.5, Degrees); + + return run(coroutine -> { + motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, targetRads); + + coroutine.waitUntil(() -> Math.abs(motor.getEncoderDistance() - targetRads) <= tolerance); + }) + .named("Intake.Wrist.MoveToAngle[" + target.toLongString() + "]"); + } + + /** + * Moves the wrist to a specific angle. The command will hold the wrist at the target angle until + * interrupted. + * + * @param target The target angle to hold + * @return A command that holds the wrist at the target angle + */ + public Command holdAngle(Angle target) { + double targetRads = target.in(Radians); + + return run(coroutine -> { + motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, targetRads); + coroutine.park(); + }) + .named("Intake.Wrist.HoldAngle[" + target.toLongString() + "]"); + } + + /** + * Moves the wrist to the stowed position and holds it there until interrupted. + * + * @return A command that stows the wrist + */ + public Command stow() { + return holdAngle(IntakeConstants.WRIST_STOW_ANGLE); + } + + /** + * Moves the wrist to the down position and holds it there until interrupted. + * + * @return A command that moves the wrist to the down position + */ + public Command down() { + return holdAngle(IntakeConstants.WRIST_DOWN_ANGLE); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.java new file mode 100644 index 00000000000..b459d622f9a --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.java @@ -0,0 +1,145 @@ +// 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.rebuiltcmdv3.mechanisms; + +import static org.wpilib.units.Units.Degrees; +import static org.wpilib.units.Units.Meters; +import static org.wpilib.units.Units.RPM; +import static org.wpilib.units.Units.Radians; + +import java.util.function.Supplier; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.ShooterConstants; +import org.wpilib.examples.rebuiltcmdv3.lookup.LookupTables; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; +import org.wpilib.math.interpolation.InterpolatingTreeMap; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; +import org.wpilib.units.measure.Distance; + +@Logged +public class Shooter implements Mechanism { + private static final InterpolatingTreeMap HOOD_LUT = + new InterpolatingTreeMap<>( + LookupTables.inverseUnitInterpolator(), LookupTables.unitInterpolator()); + private static final InterpolatingTreeMap FLYWHEEL_LUT = + new InterpolatingTreeMap<>( + LookupTables.inverseUnitInterpolator(), LookupTables.unitInterpolator()); + + static { + // Populate the lookup tables + HOOD_LUT.put(Meters.of(0), Degrees.of(0)); + HOOD_LUT.put(Meters.of(1), Degrees.of(5)); + HOOD_LUT.put(Meters.of(2), Degrees.of(10)); + HOOD_LUT.put(Meters.of(3), Degrees.of(20)); + HOOD_LUT.put(Meters.of(4), Degrees.of(30)); + + FLYWHEEL_LUT.put(Meters.of(0), RPM.of(1000)); + FLYWHEEL_LUT.put(Meters.of(1), RPM.of(1000)); + FLYWHEEL_LUT.put(Meters.of(2), RPM.of(2000)); + FLYWHEEL_LUT.put(Meters.of(3), RPM.of(3000)); + FLYWHEEL_LUT.put(Meters.of(4), RPM.of(4000)); + } + + private final ExampleSmartMotorController hoodMotor; + + private final ExampleSmartMotorController flywheelMotorPrimary; + private final ExampleSmartMotorController flywheelMotorSecondary; + private final ExampleSmartMotorController flywheelMotorTertiary; + private final ExampleSmartMotorController flywheelMotorQuatenary; + + /** + * Creates a new shooter mechanism object and initializes its motors. Only one shooter mechanism + * should exist at a time. + */ + public Shooter() { + hoodMotor = new ExampleSmartMotorController(ShooterConstants.HOOD_MOTOR_ID); + flywheelMotorPrimary = + new ExampleSmartMotorController(ShooterConstants.PRIMARY_SHOOTER_MOTOR_ID); + flywheelMotorSecondary = + new ExampleSmartMotorController(ShooterConstants.SECONDARY_SHOOTER_MOTOR_ID); + flywheelMotorTertiary = + new ExampleSmartMotorController(ShooterConstants.TERTIARY_SHOOTER_MOTOR_ID); + flywheelMotorQuatenary = + new ExampleSmartMotorController(ShooterConstants.QUATERNARY_SHOOTER_MOTOR_ID); + + flywheelMotorSecondary.follow(flywheelMotorPrimary); + flywheelMotorTertiary.follow(flywheelMotorPrimary); + flywheelMotorQuatenary.follow(flywheelMotorPrimary); + + hoodMotor.setPID(ShooterConstants.HOOD_KP, 0, 0); + flywheelMotorPrimary.setPID(ShooterConstants.FLYWHEEL_KP, 0, 0); + + setDefaultCommand(idle()); + } + + @Override + public Command idle() { + return run(coroutine -> { + coroutine.awaitAll(runHoodAngle(Degrees::zero), runFlywheelSpeed(RPM::zero)); + }) + .withPriority(Command.LOWEST_PRIORITY) + .named("Shooter.Idle"); + } + + /** + * Shoots at the hub. The flywheel and hood are automatically adjusted based on the distance to + * the center of the hub. This could potentially be used for passing as well; but, because the + * center of the hub is six feet above the ground, the distance would need to be fudged to account + * for the height difference. + * + * @param targetDistance The distance to the target, measured from the center of the robot to the + * center of the target. + * @return A command that shoots at the hub + */ + public Command shootAtHub(Supplier targetDistance) { + // Because this command is used when only the distance is known, + // we need to do a little bit of work to convert from a target distance to + // individual hood angle and flywheel speed values. + Supplier hoodAngle = () -> HOOD_LUT.get(targetDistance.get()); + Supplier flywheelSpeed = () -> FLYWHEEL_LUT.get(targetDistance.get()); + + return run(coroutine -> { + coroutine.awaitAll(runHoodAngle(hoodAngle), runFlywheelSpeed(flywheelSpeed)); + }) + .named("Shooter.ShootAtHub"); + } + + // Private commands to be used by compositions + + /** + * Moves the hood to the specified angle and holds it. This command will run forever unless + * interrupted. + * + * @param angle A dynamic supplier for the desired hood angle. + * @return The hood angle command + */ + private Command runHoodAngle(Supplier angle) { + return runRepeatedly( + () -> { + hoodMotor.setSetpoint( + ExampleSmartMotorController.PIDMode.kPosition, angle.get().in(Radians)); + }) + .named("Shooter.RunHoodAngle"); + } + + /** + * Runs the flywheel at the specified speed and holds it. This command will run forever unless + * interrupted. + * + * @param speed A dynamic supplier for the desired flywheel speed. + * @return The flywheel speed command + */ + private Command runFlywheelSpeed(Supplier speed) { + return runRepeatedly( + () -> { + flywheelMotorPrimary.setSetpoint( + ExampleSmartMotorController.PIDMode.kVelocity, speed.get().in(RPM)); + }) + .named("Shooter.RunFlywheelSpeed"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.java new file mode 100644 index 00000000000..e0d24aeb0b3 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.java @@ -0,0 +1,242 @@ +// 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.rebuiltcmdv3.mechanisms; + +import static org.wpilib.units.Units.MetersPerSecond; +import static org.wpilib.units.Units.Radians; +import static org.wpilib.units.Units.RadiansPerSecond; + +import java.util.function.Function; +import java.util.function.Supplier; +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.constants.DriveConstants; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; +import org.wpilib.examples.rebuiltcmdv3.stubs.PathFollower; +import org.wpilib.hardware.imu.OnboardIMU; +import org.wpilib.math.controller.PIDController; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.ChassisVelocities; +import org.wpilib.math.kinematics.SwerveDriveKinematics; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleVelocity; +import org.wpilib.units.measure.Angle; +import org.wpilib.units.measure.AngularVelocity; + +@Logged +public class SwerveDrive implements Mechanism { + public final SwerveDriveKinematics kinematics = DriveConstants.KINEMATICS; + private final OnboardIMU gyro = new OnboardIMU(OnboardIMU.MountOrientation.FLAT); + private final SwerveModule frontLeft = + new SwerveModule( + new ExampleSmartMotorController(DriveConstants.FRONT_LEFT_DRIVE_ID), + new ExampleSmartMotorController(DriveConstants.FRONT_LEFT_TURN_ID)); + private final SwerveModule frontRight = + new SwerveModule( + new ExampleSmartMotorController(DriveConstants.FRONT_RIGHT_DRIVE_ID), + new ExampleSmartMotorController(DriveConstants.FRONT_RIGHT_TURN_ID)); + private final SwerveModule rearLeft = + new SwerveModule( + new ExampleSmartMotorController(DriveConstants.REAR_LEFT_DRIVE_ID), + new ExampleSmartMotorController(DriveConstants.REAR_LEFT_TURN_ID)); + private final SwerveModule rearRight = + new SwerveModule( + new ExampleSmartMotorController(DriveConstants.REAR_RIGHT_DRIVE_ID), + new ExampleSmartMotorController(DriveConstants.REAR_RIGHT_TURN_ID)); + + // PID controllers for XY field position and heading + // These accept position (meters for XY, radians for heading) and output velocities + // (meters per second for XY, radians per second for heading) + private final PIDController xController = new PIDController(0.05, 0, 0); + private final PIDController yController = new PIDController(0.05, 0, 0); + private final PIDController headingController = new PIDController(0.05, 0, 0); + + public SwerveDrive() { + // Enable continuous input on the heading PID controller to handle wraparound. + headingController.enableContinuousInput(-Math.PI, Math.PI); + } + + /** + * Gets the heading of the drivebase. Used for localization. + * + * @return The heading of the drivebase. + */ + public Rotation2d getGyroHeading() { + return gyro.getRotation2d(); + } + + /** + * Gets the positions of all swerve modules. Used for localization. + * + * @return An array of module positions. + */ + public SwerveModulePosition[] getModulePositions() { + return getSwerveData(SwerveModule::getPosition); + } + + public SwerveModuleVelocity[] getModuleVelocities() { + return getSwerveData(SwerveModule::getVelocity); + } + + @Override + public Command idle() { + return runRepeatedly( + () -> { + frontLeft.stop(); + frontRight.stop(); + rearLeft.stop(); + rearRight.stop(); + }) + .named("Drive.Idle"); + } + + /** + * A command that gives the driver full control of the drive. + * + * @param controller The driver controller. + * @return a command that gives the driver full control of the drive + */ + public Command driverControl(CommandGamepad controller) { + return driveFieldRelative( + () -> { + double x = -controller.getLeftY(); + double y = -controller.getLeftX(); + double omega = -controller.getRightX(); + + return new ChassisVelocities( + DriveConstants.MAX_VELOCITY.times(x), + DriveConstants.MAX_VELOCITY.times(y), + DriveConstants.MAX_TURN_RATE.times(omega)); + }); + } + + /** + * A command that aims the drivebase at a target. The command will exit when the necessary heading + * is reached. + * + * @param robotPose A supplier for the current pose of the robot. + * @param targetPose A supplier for the target pose to aim at. + * @return The aiming command + */ + public Command aimAt(Supplier robotPose, Pose2d targetPose) { + return driveFieldRelative( + () -> { + Pose2d robot = robotPose.get(); + Angle targetHeading = angleBetweenPoses(robot, targetPose); + AngularVelocity omega = + RadiansPerSecond.of( + headingController.calculate( + robot.getRotation().getRadians(), targetHeading.in(Radians))); + + return new ChassisVelocities(MetersPerSecond.of(0), MetersPerSecond.of(0), omega); + }) + .until(headingController::atSetpoint) + .named("Drive.AimAt"); + } + + /** + * A command that assists the driver in aiming the robot at a target pose. The driver still has + * full control over the X and Y speeds, but heading is controlled automatically based on the + * relative position of the robot and the target. + * + * @param controller The driver controller. + * @param robotPose A supplier for the current robot pose. + * @param targetPose A supplier for the target pose. + * @return The aim-assist command. + */ + public Command aimAssist( + CommandGamepad controller, Supplier robotPose, Supplier targetPose) { + return driveFieldRelative( + () -> { + // We use the driver's input to control X and Y speed as normal... + double x = -controller.getLeftY(); + double y = -controller.getLeftX(); + + // ... but heading is controlled automatically based on the relative position of the robot + // and + // the target. + Pose2d robot = robotPose.get(); + Pose2d target = targetPose.get(); + Angle targetHeading = angleBetweenPoses(robot, target); + AngularVelocity omega = + RadiansPerSecond.of( + headingController.calculate( + robot.getRotation().getRadians(), targetHeading.in(Radians))); + + return new ChassisVelocities( + DriveConstants.MAX_VELOCITY.times(x), DriveConstants.MAX_VELOCITY.times(y), omega); + }); + } + + /** + * Computes the angle from one pose to another. Useful for aiming the robot (positioned at the + * source pose) at some target point on the field. + * + * @param source The source pose + * @param target The target pose + * @return The angle between the two poses + */ + private static Angle angleBetweenPoses(Pose2d source, Pose2d target) { + // atan2 conveniently returns a value in radians between -pi and pi, so we don't need to do + // any additional calculations to normalize the angle + return Radians.of(Math.atan2(target.getY() - source.getY(), target.getX() - source.getX())); + } + + /** + * Drives with a dynamic velocity. This command will never exit. + * + * @param velocities A supplier of chassis velocities. These velocities are expected to be in the + * field coordinate system. + * @return a command that drives with the given velocities + */ + public Command driveFieldRelative(Supplier velocities) { + return runRepeatedly( + () -> { + ChassisVelocities velocity = velocities.get().toRobotRelative(getGyroHeading()); + SwerveModuleVelocity[] swerveModuleVelocities = + kinematics.toSwerveModuleVelocities(velocity); + + frontLeft.setTargetVelocity(swerveModuleVelocities[0]); + frontRight.setTargetVelocity(swerveModuleVelocities[1]); + rearLeft.setTargetVelocity(swerveModuleVelocities[2]); + rearRight.setTargetVelocity(swerveModuleVelocities[3]); + }) + .named("Drive.DriveSpeeds"); + } + + /** + * Follows a path loaded from a file. The command will exit when the robot reaches the end of the + * path. + * + * @param pathName the path to follow + * @return a command that follows the path + */ + public Command followPath(String pathName) { + return run(coroutine -> { + // Load the path + PathFollower follower = PathFollower.load(pathName); + + // Run path follower in the background + coroutine.fork(driveFieldRelative(follower::next)); + + // Wait until we reach the end of the path, then exit + coroutine.waitUntil(follower::isDone); + }) + .named("Drive.FollowPath[" + pathName + "]"); + } + + @SuppressWarnings("unchecked") + private T[] getSwerveData(Function extractor) { + T[] data = (T[]) new Object[4]; + data[0] = extractor.apply(frontLeft); + data[1] = extractor.apply(frontRight); + data[2] = extractor.apply(rearLeft); + data[3] = extractor.apply(rearRight); + return data; + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java new file mode 100644 index 00000000000..59afc7cdc93 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java @@ -0,0 +1,66 @@ +// 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.rebuiltcmdv3.mechanisms; + +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.kinematics.SwerveModulePosition; +import org.wpilib.math.kinematics.SwerveModuleVelocity; + +@Logged +public final class SwerveModule { + private final ExampleSmartMotorController driveMotor; + private final ExampleSmartMotorController turnMotor; + + /** + * Creates a new swerve module with the given drive and turn motors. This constructor is + * package-private and should only be called by the SwerveDrive class. + * + * @param driveMotor The motor that drives the module. + * @param turnMotor The motor that turns the module. + */ + SwerveModule(ExampleSmartMotorController driveMotor, ExampleSmartMotorController turnMotor) { + this.driveMotor = driveMotor; + this.turnMotor = turnMotor; + } + + /** + * Sets the target velocity of this module. + * + * @param targetVelocity The target velocity of the module. + */ + public void setTargetVelocity(SwerveModuleVelocity targetVelocity) { + driveMotor.setSetpoint(ExampleSmartMotorController.PIDMode.kVelocity, targetVelocity.velocity); + turnMotor.setSetpoint( + ExampleSmartMotorController.PIDMode.kPosition, targetVelocity.angle.getRadians()); + } + + /** Stops the module by turning off the drive and turn motors. */ + public void stop() { + driveMotor.stopMotor(); + turnMotor.stopMotor(); + } + + /** + * Gets the current position of the module. + * + * @return The current position of the module. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + driveMotor.getEncoderDistance(), Rotation2d.fromRadians(turnMotor.getEncoderDistance())); + } + + /** + * Gets the current velocity of the module. + * + * @return The current velocity of the module. + */ + public SwerveModuleVelocity getVelocity() { + return new SwerveModuleVelocity( + driveMotor.getEncoderRate(), Rotation2d.fromRadians(turnMotor.getEncoderDistance())); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java new file mode 100644 index 00000000000..4aabced4d1e --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java @@ -0,0 +1,22 @@ +// 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.rebuiltcmdv3.opmodes.auto; + +import org.wpilib.examples.rebuiltcmdv3.Robot; +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.OpMode; + +@Autonomous +public class DoNothingAuto implements OpMode { + /** + * Creates a new autonomous opmode that does nothing. This constructor is called automatically by + * the OpModeRobot framework when the program starts up. + * + * @param robot The robot instance to control. + */ + public DoNothingAuto(Robot robot) { + // Robot mechanisms have idle default commands, so we don't need to do anything special + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java new file mode 100644 index 00000000000..ea282eaddd1 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java @@ -0,0 +1,60 @@ +// 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.rebuiltcmdv3.opmodes.auto; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Trigger; +import org.wpilib.driverstation.RobotState; +import org.wpilib.examples.rebuiltcmdv3.Robot; +import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants; +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.OpMode; + +@Autonomous +public class SweepAuto implements OpMode { + private final Trigger enabled = new Trigger(RobotState::isEnabled); + + /** + * Creates a new autonomous mode that sweeps the left trench and scores on the hub. This + * constructor is called automatically by the OpModeRobot framework when the program starts up. + * + * @param robot The robot instance to control. + */ + public SweepAuto(Robot robot) { + // Start the intake stowed + robot.intake.setDefaultCommand(robot.intake.stow()); + + // Extend the intake once we cross into the neutral zone and leave it extended for the entire + // opmode + robot.inNeutralZone.onTrue(robot.intake.intake()); + + // Once the robot is enabled, start following a sweep path through the left trench, + // into the neutral zone, then back over the bump. + // When we return to the alliance zone, aim at the hub and start shooting. + enabled.onTrue(sweepAndScore(robot)); + } + + /** + * Creates a command that drives the robot in a path that sweeps through the neutral zone to + * gather fuel, returns to the alliance zone, and then shoots at the hub until the end of the + * autonomous period. + * + * @param robot The robot instance to control + * @return A sweep-and-score command + */ + private Command sweepAndScore(Robot robot) { + return Command.noRequirements( + coroutine -> { + coroutine.await(robot.swerveDrive.followPath("nz-sweep-left-trench")); + + FieldConstants.targetHub() + .ifPresent( + hub -> { + coroutine.await(robot.shootAt(hub)); + }); + }) + .named("Sweep and Score"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java new file mode 100644 index 00000000000..c5f45491ffd --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java @@ -0,0 +1,40 @@ +// 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.rebuiltcmdv3.opmodes.teleop; + +import org.wpilib.examples.rebuiltcmdv3.Robot; +import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants; +import org.wpilib.opmode.OpMode; +import org.wpilib.opmode.Teleop; + +@Teleop +public class DefaultTeleop implements OpMode { + /** + * Creates a new default teleop mode. This constructor is called automatically by the OpModeRobot + * framework when the program starts up. + * + * @param robot The robot instance to control. + */ + public DefaultTeleop(Robot robot) { + robot.swerveDrive.setDefaultCommand(robot.swerveDrive.driverControl(robot.controller)); + + robot.controller.faceUp().onTrue(robot.intake.intake()); + robot.controller.faceDown().onTrue(robot.intake.stow()); + robot.controller.faceRight().whileTrue(robot.intake.agitate()); + + // The right stick is used for turning, but switch to aim assist when the driver presses down + // on the stick. Un-pressing the stick will return to normal driver control. + // If, for some reason, no target hub is found, the robot's current pose will be used as the + // target location. + robot + .controller + .rightStick() + .whileTrue( + robot.swerveDrive.aimAssist( + robot.controller, + robot.poseEstimator::getEstimatedPose, + () -> FieldConstants.targetHub().orElseGet(robot.poseEstimator::getEstimatedPose))); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.java new file mode 100644 index 00000000000..8d9518d7148 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.java @@ -0,0 +1,97 @@ +// 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.rebuiltcmdv3.stubs; + +/** + * A simplified stub class that simulates the API of a common "smart" motor controller. + * + *

Has no actual functionality. + */ +public class ExampleSmartMotorController { + public enum PIDMode { + kPosition, + kVelocity, + kMovementWitchcraft + } + + double value; + + /** + * Creates a new ExampleSmartMotorController. + * + * @param port The port for the controller. + */ + public ExampleSmartMotorController(int port) {} + + /** + * Example method for setting the PID gains of the smart controller. + * + * @param kp The proportional gain. + * @param ki The integral gain. + * @param kd The derivative gain. + */ + public void setPID(double kp, double ki, double kd) {} + + /** + * Example method for setting the setpoint of the smart controller in PID mode. + * + * @param mode The mode of the PID controller. + * @param setpoint The controller setpoint. + */ + public void setSetpoint(PIDMode mode, double setpoint) {} + + /** + * Example method for setting the voltage supplied to the motor. + * + * @param volts How many volts to apply to the motor. + */ + public void setVoltage(int volts) {} + + /** + * Places this motor controller in follower mode. + * + * @param leader The leader to follow. + */ + public void follow(ExampleSmartMotorController leader) {} + + /** + * Returns the encoder distance. + * + * @return The current encoder distance. + */ + public double getEncoderDistance() { + return 0; + } + + /** + * Returns the encoder rate. + * + * @return The current encoder rate. + */ + public double getEncoderRate() { + return 0; + } + + /** Resets the encoder to zero distance. */ + public void resetEncoder() {} + + public void setThrottle(double throttle) { + value = throttle; + } + + public double getThrottle() { + return value; + } + + public void setInverted(boolean isInverted) {} + + public boolean getInverted() { + return false; + } + + public void disable() {} + + public void stopMotor() {} +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.java new file mode 100644 index 00000000000..b0109674ae9 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.java @@ -0,0 +1,49 @@ +// 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.rebuiltcmdv3.stubs; + +import org.wpilib.math.kinematics.ChassisVelocities; + +/** + * A stub interface for an API that could plausibly follow a path. No WPILib code exists for this; + * if you want to follow paths on a real robot, use a third-party library provided by the FRC + * programming community. + */ +public interface PathFollower { + /** + * Gets the next chassis velocities from the path. + * + * @return the next chassis velocities + */ + ChassisVelocities next(); + + /** + * Checks if the robot has reached the end of the path. + * + * @return true if the robot has reached the end of the path, false otherwise + */ + boolean isDone(); + + /** + * Loads a path from a file in the deploy directory. This is a stub implementation as an example; + * a real robot program would use a third-party path following library. + * + * @param pathName The name of the path file to load. + * @return A PathFollower instance that can follow the loaded path. + */ + static PathFollower load(String pathName) { + return new PathFollower() { + @Override + public ChassisVelocities next() { + return new ChassisVelocities(); + } + + @Override + public boolean isDone() { + return true; + } + }; + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleAuto.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleAuto.java new file mode 100644 index 00000000000..aa1115ad3b3 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleAuto.java @@ -0,0 +1,15 @@ +// 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.templates.commandv3; + +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.OpMode; + +@Autonomous +public class ExampleAuto implements OpMode { + public ExampleAuto(Robot robot) { + robot.exampleMechanism.exampleCondition.whileTrue(robot.exampleMechanism.exampleCommand()); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java new file mode 100644 index 00000000000..5d9defbcd52 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java @@ -0,0 +1,15 @@ +// 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.templates.commandv3; + +import org.wpilib.opmode.OpMode; +import org.wpilib.opmode.Teleop; + +@Teleop +public class ExampleTeleop implements OpMode { + public ExampleTeleop(Robot robot) { + robot.exampleController.rightStick().whileTrue(robot.exampleMechanism.exampleCommand()); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java new file mode 100644 index 00000000000..7ff679f08c0 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java @@ -0,0 +1,24 @@ +// 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.templates.commandv3; + +import org.wpilib.command3.Scheduler; +import org.wpilib.command3.button.CommandGamepad; +import org.wpilib.framework.OpModeRobot; +import org.wpilib.templates.commandv3.constants.DriverConstants; +import org.wpilib.templates.commandv3.mechanisms.ExampleMechanism; + +public class Robot extends OpModeRobot { + final ExampleMechanism exampleMechanism = new ExampleMechanism(); + final CommandGamepad exampleController = + new CommandGamepad(DriverConstants.DRIVER_CONTROLLER_PORT); + + public Robot() {} + + @Override + public void robotPeriodic() { + Scheduler.getDefault().run(); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/DriverConstants.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/DriverConstants.java new file mode 100644 index 00000000000..35333ad0893 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/DriverConstants.java @@ -0,0 +1,9 @@ +// 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.templates.commandv3.constants; + +public final class DriverConstants { + public static final int DRIVER_CONTROLLER_PORT = 1; +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/ExampleConstants.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/ExampleConstants.java new file mode 100644 index 00000000000..6a9d319af29 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/ExampleConstants.java @@ -0,0 +1,10 @@ +// 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.templates.commandv3.constants; + +import org.wpilib.templates.commandv3.mechanisms.ExampleMechanism; + +/** Constants for the {@link ExampleMechanism}. */ +public final class ExampleConstants {} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/mechanisms/ExampleMechanism.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/mechanisms/ExampleMechanism.java new file mode 100644 index 00000000000..59852f51d67 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/mechanisms/ExampleMechanism.java @@ -0,0 +1,34 @@ +// 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.templates.commandv3.mechanisms; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.command3.Trigger; + +/** An example class demonstrating basic use of mechanisms and commands. */ +public class ExampleMechanism implements Mechanism { + public final Trigger exampleCondition = new Trigger(() -> false); + + /** + * An example command that requires this mechanism. Only one command that requires this mechanism + * may run at a time - if a command is already running, this command will interrupt it. + * + *

This example runs indefinitely and prints a message every time it executes. + * + * @return An example command. + */ + public Command exampleCommand() { + return run(coroutine -> { + int runCount = 0; + while (true) { + runCount++; + System.out.println("Example command run #" + runCount); + coroutine.yield(); + } + }) + .named("Example Command"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java new file mode 100644 index 00000000000..f484a0f7868 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java @@ -0,0 +1,21 @@ +// 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.templates.commandv3skeleton; + +import org.wpilib.command3.Scheduler; +import org.wpilib.epilogue.Epilogue; +import org.wpilib.epilogue.Logged; +import org.wpilib.framework.OpModeRobot; + +@Logged +public class Robot extends OpModeRobot { + public Robot() {} + + @Override + public void robotPeriodic() { + Scheduler.getDefault().run(); + Epilogue.update(this); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json b/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json index 1f722369d2f..95a3d140b3d 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json @@ -22,6 +22,29 @@ "robotclass": "Robot", "commandversion": 2 }, + { + "name": "Command v3 Robot", + "description": "Command v3, with explanatory comments and example code.", + "tags": [ + "Commandv3" + ], + "foldername": "commandv3", + "gradlebase": "java", + "robotclass": "Robot", + "commandversion": 3 + }, + { + "name": "Command v3 Robot Skeleton (Advanced)", + "description": "Skeleton (stub) code for Command v3, without explanatory comments and example code.", + "tags": [ + "Commandv3", + "Skeleton" + ], + "foldername": "commandv3skeleton", + "gradlebase": "java", + "robotclass": "Robot", + "commandversion": 3 + }, { "name": "OpMode Robot", "description": "OpMode style, with explanatory comments and example code.",