From 144e6bcc78799da13928d3f7bbb0b921646792af Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 10 May 2026 13:40:01 -0400 Subject: [PATCH 01/13] Add example and template projects for commands v3 --- .../examples/command3/PoseEstimator.java | 67 +++++ .../org/wpilib/examples/command3/Robot.java | 75 ++++++ .../command3/constants/DriveConstants.java | 28 ++ .../command3/constants/FieldConstants.java | 39 +++ .../command3/constants/IntakeConstants.java | 25 ++ .../command3/constants/ShooterConstants.java | 23 ++ .../command3/lookup/LookupTables.java | 57 ++++ .../examples/command3/mechanisms/Intake.java | 87 +++++++ .../command3/mechanisms/IntakeRoller.java | 67 +++++ .../command3/mechanisms/IntakeWrist.java | 83 ++++++ .../examples/command3/mechanisms/Shooter.java | 145 +++++++++++ .../command3/mechanisms/SwerveDrive.java | 244 ++++++++++++++++++ .../command3/mechanisms/SwerveModule.java | 66 +++++ .../command3/opmodes/auto/DoNothingAuto.java | 22 ++ .../command3/opmodes/auto/SweepAuto.java | 60 +++++ .../opmodes/teleop/DefaultTeleop.java | 40 +++ .../stubs/ExampleSmartMotorController.java | 97 +++++++ .../examples/command3/stubs/PathFollower.java | 49 ++++ .../java/org/wpilib/examples/examples.json | 15 ++ .../templates/commandv3/ExampleAuto.java | 15 ++ .../templates/commandv3/ExampleTeleop.java | 15 ++ .../org/wpilib/templates/commandv3/Robot.java | 24 ++ .../commandv3/constants/DriverConstants.java | 9 + .../commandv3/constants/ExampleConstants.java | 10 + .../mechanisms/ExampleMechanism.java | 34 +++ .../templates/commandv3skeleton/Robot.java | 21 ++ .../java/org/wpilib/templates/templates.json | 23 ++ 27 files changed, 1440 insertions(+) create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/DriveConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/FieldConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/IntakeConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/ShooterConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/lookup/LookupTables.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeWrist.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Shooter.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveModule.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/DoNothingAuto.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/SweepAuto.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/ExampleSmartMotorController.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/PathFollower.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleAuto.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/DriverConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/constants/ExampleConstants.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/mechanisms/ExampleMechanism.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java new file mode 100644 index 00000000000..0f7c63d47c5 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3; + +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.command3.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/command3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java new file mode 100644 index 00000000000..d63ceaf3406 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3; + +import static org.wpilib.examples.command3.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.CommandNiDsPS5Controller; +import org.wpilib.epilogue.Epilogue; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.command3.mechanisms.Intake; +import org.wpilib.examples.command3.mechanisms.Shooter; +import org.wpilib.examples.command3.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 CommandNiDsPS5Controller controller = new CommandNiDsPS5Controller(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/command3/constants/DriveConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/DriveConstants.java new file mode 100644 index 00000000000..fa9571ff543 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/constants/FieldConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/FieldConstants.java new file mode 100644 index 00000000000..152c601a8c8 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/constants/IntakeConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/IntakeConstants.java new file mode 100644 index 00000000000..0bde220c9a6 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/constants/ShooterConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/ShooterConstants.java new file mode 100644 index 00000000000..179c50fdbf4 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/lookup/LookupTables.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/lookup/LookupTables.java new file mode 100644 index 00000000000..25636c8de07 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/mechanisms/Intake.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java new file mode 100644 index 00000000000..a8493ac1d76 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java @@ -0,0 +1,87 @@ +// 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.command3.mechanisms; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Mechanism; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.command3.constants.IntakeConstants; + +@Logged +public class Intake extends 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) { + 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 Command.parallel(wrist.down(), roller.expel()).named("Intake.Outtake"); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java new file mode 100644 index 00000000000..77d036f909a --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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.command3.constants.IntakeConstants; +import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; + +@Logged +public class IntakeRoller extends 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/command3/mechanisms/IntakeWrist.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeWrist.java new file mode 100644 index 00000000000..9a7765fe68b --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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.command3.constants.IntakeConstants; +import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +import org.wpilib.units.measure.Angle; + +@Logged +public class IntakeWrist extends 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/command3/mechanisms/Shooter.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Shooter.java new file mode 100644 index 00000000000..1e6cfde4597 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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.command3.constants.ShooterConstants; +import org.wpilib.examples.command3.lookup.LookupTables; +import org.wpilib.examples.command3.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 extends 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/command3/mechanisms/SwerveDrive.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java new file mode 100644 index 00000000000..75efa6d145f --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java @@ -0,0 +1,244 @@ +// 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.command3.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.CommandNiDsPS5Controller; +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.command3.constants.DriveConstants; +import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +import org.wpilib.examples.command3.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 extends 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(CommandNiDsPS5Controller 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( + CommandNiDsPS5Controller 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/command3/mechanisms/SwerveModule.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveModule.java new file mode 100644 index 00000000000..1c12732fc5d --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.mechanisms; + +import org.wpilib.epilogue.Logged; +import org.wpilib.examples.command3.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/command3/opmodes/auto/DoNothingAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/DoNothingAuto.java new file mode 100644 index 00000000000..b2aab508314 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.opmodes.auto; + +import org.wpilib.examples.command3.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/command3/opmodes/auto/SweepAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/SweepAuto.java new file mode 100644 index 00000000000..eecf826d105 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.opmodes.auto; + +import org.wpilib.command3.Command; +import org.wpilib.command3.Trigger; +import org.wpilib.driverstation.RobotState; +import org.wpilib.examples.command3.Robot; +import org.wpilib.examples.command3.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/command3/opmodes/teleop/DefaultTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java new file mode 100644 index 00000000000..57bd323be81 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.opmodes.teleop; + +import org.wpilib.examples.command3.Robot; +import org.wpilib.examples.command3.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.circle().onTrue(robot.intake.intake()); + robot.controller.cross().onTrue(robot.intake.stow()); + robot.controller.square().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 + .R3() + .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/command3/stubs/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/ExampleSmartMotorController.java new file mode 100644 index 00000000000..63b2824ac9c --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/command3/stubs/PathFollower.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/PathFollower.java new file mode 100644 index 00000000000..046a6f498c9 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/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.command3.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/examples/examples.json b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json index 4d7839cfde1..49dc3afa0e3 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json @@ -10,6 +10,21 @@ "robotclass": "Robot", "commandversion": 2 }, + { + "name": "Commands v3", + "description": "A robot with a drivetrain, intake, and shooter subsystems controlled by the commands v3 framework", + "tags": [ + "Complete Robot", + "Commandv3", + "DataLog", + "Joystick", + "Swerve Drive" + ], + "foldername": "command3", + "gradlebase": "java", + "robotclass": "Robot", + "commandversion": 3 + }, { "name": "Encoder", "description": "View values from a quadrature encoder.", 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..2b736c38a85 --- /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.R1().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..48ff30e6219 --- /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.CommandNiDsPS5Controller; +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 CommandNiDsPS5Controller exampleController = + new CommandNiDsPS5Controller(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..4775807a781 --- /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 extends 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.", From 7dd193d56131fb47f746dbd86b1411b5afd98f12 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 10 May 2026 13:58:55 -0400 Subject: [PATCH 02/13] Rewrite hatchbot v3 example to be idiomatic --- .../examples/hatchbotcmdv3/Constants.java | 2 +- .../wpilib/examples/hatchbotcmdv3/Robot.java | 80 +++++++++++----- .../hatchbotcmdv3/RobotContainer.java | 96 ------------------- .../hatchbotcmdv3/commands/Autos.java | 71 +++----------- .../DriveMechanism.java} | 55 +++++++++-- .../HatchMechanism.java} | 4 +- 6 files changed, 120 insertions(+), 188 deletions(-) delete mode 100644 wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/RobotContainer.java rename wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/{subsystems/DriveSubsystem.java => mechanisms/DriveMechanism.java} (64%) rename wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotcmdv3/{subsystems/HatchSubsystem.java => mechanisms/HatchMechanism.java} (93%) 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..539ba7d11ef 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.eastFace().onTrue(hatchMechanism.grabHatchCommand()); + // Release the hatch when the Square button is pressed. + driverController.westFace().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 de773496616..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.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 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 d8056e96d54..9706ede1196 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 extends Mechanism { +public class DriveMechanism 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); @@ -38,8 +40,8 @@ public class DriveSubsystem extends 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 3790748d22e..f1a1c7fe89b 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 extends Mechanism { +public class HatchMechanism extends Mechanism { private final DoubleSolenoid hatchSolenoid = new DoubleSolenoid( 0, From b7e8392308baa7451235e401e91abcf98482de0c Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 10 May 2026 14:28:17 -0400 Subject: [PATCH 03/13] Update bazel lists --- wpilibcExamples/example_projects.bzl | 3 +++ 1 file changed, 3 insertions(+) diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index 79307ef6b99..7abbda0452d 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -1,6 +1,7 @@ EXAMPLE_FOLDERS = [ "ArcadeDriveGamepad", "ArmSimulation", + "Commandv3", "DifferentialDriveBot", "DifferentialDrivePoseEstimator", "DriveDistanceOffboard", @@ -82,6 +83,8 @@ SNIPPET_FOLDERS = [ TEMPLATE_FOLDERS = [ "commandv2", "commandv2skeleton", + "commandv3", + "commandv3skeleton", "opmode", "robotbaseskeleton", "timed", From bf5bd8a5dff7684e2fb819c4acd429116782d12f Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 17 May 2026 12:22:34 -0400 Subject: [PATCH 04/13] Use correct ownership of inner mechanisms --- .../java/org/wpilib/examples/command3/mechanisms/Intake.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java index a8493ac1d76..d6933ac753b 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java @@ -82,6 +82,9 @@ public Command agitate() { * @return A command that expels fuel from the intake mechanism */ public Command outtake() { - return Command.parallel(wrist.down(), roller.expel()).named("Intake.Outtake"); + return run(coroutine -> { + coroutine.awaitAll(wrist.down(), roller.expel()); + }) + .named("Intake.Outtake"); } } From 37224eb2e4b280938300b57bd4810907c3c84bac Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 17 May 2026 12:26:52 -0400 Subject: [PATCH 05/13] Describe why awaiting in loops doesn't need explicit yields --- .../java/org/wpilib/examples/command3/mechanisms/Intake.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java index d6933ac753b..f671c20d2f8 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java @@ -69,6 +69,8 @@ public Command agitate() { 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)); } From 8a1d48f9e1aaab410daab7d8f1208929098f213b Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 17 May 2026 12:33:26 -0400 Subject: [PATCH 06/13] Use generic CommandGamepad --- .../src/main/java/org/wpilib/examples/command3/Robot.java | 4 ++-- .../wpilib/examples/command3/mechanisms/SwerveDrive.java | 8 +++----- .../examples/command3/opmodes/teleop/DefaultTeleop.java | 8 ++++---- 3 files changed, 9 insertions(+), 11 deletions(-) diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java index d63ceaf3406..8b67887f6f0 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java @@ -10,7 +10,7 @@ import org.wpilib.command3.Command; import org.wpilib.command3.Scheduler; import org.wpilib.command3.Trigger; -import org.wpilib.command3.button.CommandNiDsPS5Controller; +import org.wpilib.command3.button.CommandGamepad; import org.wpilib.epilogue.Epilogue; import org.wpilib.epilogue.Logged; import org.wpilib.examples.command3.mechanisms.Intake; @@ -25,7 +25,7 @@ public class Robot extends OpModeRobot { public final Shooter shooter = new Shooter(); public final Intake intake = new Intake(); public final PoseEstimator poseEstimator = new PoseEstimator(); - public final CommandNiDsPS5Controller controller = new CommandNiDsPS5Controller(1); + public final CommandGamepad controller = new CommandGamepad(1); public final Trigger inNeutralZone = new Trigger(() -> poseEstimator.inZone(NEUTRAL_ZONE)); diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java index 75efa6d145f..851cfaaf593 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java @@ -12,7 +12,7 @@ import java.util.function.Supplier; import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; -import org.wpilib.command3.button.CommandNiDsPS5Controller; +import org.wpilib.command3.button.CommandGamepad; import org.wpilib.epilogue.Logged; import org.wpilib.examples.command3.constants.DriveConstants; import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; @@ -101,7 +101,7 @@ public Command idle() { * @param controller The driver controller. * @return a command that gives the driver full control of the drive */ - public Command driverControl(CommandNiDsPS5Controller controller) { + public Command driverControl(CommandGamepad controller) { return driveFieldRelative( () -> { double x = -controller.getLeftY(); @@ -150,9 +150,7 @@ public Command aimAt(Supplier robotPose, Pose2d targetPose) { * @return The aim-assist command. */ public Command aimAssist( - CommandNiDsPS5Controller controller, - Supplier robotPose, - Supplier targetPose) { + CommandGamepad controller, Supplier robotPose, Supplier targetPose) { return driveFieldRelative( () -> { // We use the driver's input to control X and Y speed as normal... diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java index 57bd323be81..2db5237e715 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java @@ -20,9 +20,9 @@ public class DefaultTeleop implements OpMode { public DefaultTeleop(Robot robot) { robot.swerveDrive.setDefaultCommand(robot.swerveDrive.driverControl(robot.controller)); - robot.controller.circle().onTrue(robot.intake.intake()); - robot.controller.cross().onTrue(robot.intake.stow()); - robot.controller.square().whileTrue(robot.intake.agitate()); + robot.controller.northFace().onTrue(robot.intake.intake()); + robot.controller.southFace().onTrue(robot.intake.stow()); + robot.controller.eastFace().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. @@ -30,7 +30,7 @@ public DefaultTeleop(Robot robot) { // target location. robot .controller - .R3() + .rightStick() .whileTrue( robot.swerveDrive.aimAssist( robot.controller, From 981be7823e7c63d09d7b5f5e0772ffe3db90d206 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sun, 17 May 2026 12:37:57 -0400 Subject: [PATCH 07/13] Improve v3 example project naming Now "Commands v3 - REBUILT Robot" Moved to `rebuiltcmdv3` package Add "OpMode" tag --- .../src/main/java/org/wpilib/examples/examples.json | 7 ++++--- .../{command3 => rebuiltcmdv3}/PoseEstimator.java | 4 ++-- .../examples/{command3 => rebuiltcmdv3}/Robot.java | 10 +++++----- .../constants/DriveConstants.java | 2 +- .../constants/FieldConstants.java | 2 +- .../constants/IntakeConstants.java | 2 +- .../constants/ShooterConstants.java | 2 +- .../lookup/LookupTables.java | 2 +- .../{command3 => rebuiltcmdv3}/mechanisms/Intake.java | 4 ++-- .../mechanisms/IntakeRoller.java | 6 +++--- .../mechanisms/IntakeWrist.java | 6 +++--- .../{command3 => rebuiltcmdv3}/mechanisms/Shooter.java | 8 ++++---- .../mechanisms/SwerveDrive.java | 8 ++++---- .../mechanisms/SwerveModule.java | 4 ++-- .../opmodes/auto/DoNothingAuto.java | 4 ++-- .../opmodes/auto/SweepAuto.java | 6 +++--- .../opmodes/teleop/DefaultTeleop.java | 6 +++--- .../stubs/ExampleSmartMotorController.java | 2 +- .../{command3 => rebuiltcmdv3}/stubs/PathFollower.java | 2 +- 19 files changed, 44 insertions(+), 43 deletions(-) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/PoseEstimator.java (95%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/Robot.java (89%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/constants/DriveConstants.java (95%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/constants/FieldConstants.java (96%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/constants/IntakeConstants.java (94%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/constants/ShooterConstants.java (94%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/lookup/LookupTables.java (97%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/Intake.java (95%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/IntakeRoller.java (90%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/IntakeWrist.java (92%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/Shooter.java (95%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/SwerveDrive.java (97%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/mechanisms/SwerveModule.java (94%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/opmodes/auto/DoNothingAuto.java (87%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/opmodes/auto/SweepAuto.java (92%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/opmodes/teleop/DefaultTeleop.java (89%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/stubs/ExampleSmartMotorController.java (97%) rename wpilibjExamples/src/main/java/org/wpilib/examples/{command3 => rebuiltcmdv3}/stubs/PathFollower.java (96%) diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json index 49dc3afa0e3..89dc277fe83 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/examples.json @@ -11,16 +11,17 @@ "commandversion": 2 }, { - "name": "Commands v3", + "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" + "Swerve Drive", + "OpMode" ], - "foldername": "command3", + "foldername": "rebuiltcmdv3", "gradlebase": "java", "robotclass": "Robot", "commandversion": 3 diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java similarity index 95% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java index 0f7c63d47c5..5bebb934895 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/PoseEstimator.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/PoseEstimator.java @@ -2,10 +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.command3; +package org.wpilib.examples.rebuiltcmdv3; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.constants.DriveConstants; +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; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java similarity index 89% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java index 8b67887f6f0..9950adb7b5c 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/Robot.java @@ -2,9 +2,9 @@ // 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.command3; +package org.wpilib.examples.rebuiltcmdv3; -import static org.wpilib.examples.command3.constants.FieldConstants.NEUTRAL_ZONE; +import static org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants.NEUTRAL_ZONE; import static org.wpilib.units.Units.Meters; import org.wpilib.command3.Command; @@ -13,9 +13,9 @@ import org.wpilib.command3.button.CommandGamepad; import org.wpilib.epilogue.Epilogue; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.mechanisms.Intake; -import org.wpilib.examples.command3.mechanisms.Shooter; -import org.wpilib.examples.command3.mechanisms.SwerveDrive; +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; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/DriveConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.java similarity index 95% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/DriveConstants.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.java index fa9571ff543..99db87f2666 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/DriveConstants.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/DriveConstants.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.command3.constants; +package org.wpilib.examples.rebuiltcmdv3.constants; import static org.wpilib.units.Units.FeetPerSecond; import static org.wpilib.units.Units.RotationsPerSecond; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/FieldConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.java similarity index 96% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/FieldConstants.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.java index 152c601a8c8..09e01d9a5fe 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/FieldConstants.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/FieldConstants.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.command3.constants; +package org.wpilib.examples.rebuiltcmdv3.constants; import java.util.Optional; import org.wpilib.driverstation.internal.DriverStationBackend; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/IntakeConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.java similarity index 94% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/IntakeConstants.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.java index 0bde220c9a6..f333d3f892b 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/IntakeConstants.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/IntakeConstants.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.command3.constants; +package org.wpilib.examples.rebuiltcmdv3.constants; import static org.wpilib.units.Units.Degrees; import static org.wpilib.units.Units.RPM; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/ShooterConstants.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.java similarity index 94% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/ShooterConstants.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.java index 179c50fdbf4..6b2c3a80c1a 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/constants/ShooterConstants.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/constants/ShooterConstants.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.command3.constants; +package org.wpilib.examples.rebuiltcmdv3.constants; public final class ShooterConstants { public static final int HOOD_MOTOR_ID = 10; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/lookup/LookupTables.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.java similarity index 97% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/lookup/LookupTables.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.java index 25636c8de07..3595e406f6f 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/lookup/LookupTables.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/lookup/LookupTables.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.command3.lookup; +package org.wpilib.examples.rebuiltcmdv3.lookup; import static org.wpilib.units.Units.Value; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java similarity index 95% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java index f671c20d2f8..61366af04c3 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Intake.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Intake.java @@ -2,12 +2,12 @@ // 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.command3.mechanisms; +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.command3.constants.IntakeConstants; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; @Logged public class Intake extends Mechanism { diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java similarity index 90% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java index 77d036f909a..8b72b150986 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeRoller.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeRoller.java @@ -2,15 +2,15 @@ // 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.command3.mechanisms; +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.command3.constants.IntakeConstants; -import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; @Logged public class IntakeRoller extends Mechanism { diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeWrist.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.java similarity index 92% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeWrist.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.java index 9a7765fe68b..5eefb12b4f3 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/IntakeWrist.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/IntakeWrist.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.command3.mechanisms; +package org.wpilib.examples.rebuiltcmdv3.mechanisms; import static org.wpilib.units.Units.Degrees; import static org.wpilib.units.Units.Radians; @@ -10,8 +10,8 @@ import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.constants.IntakeConstants; -import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +import org.wpilib.examples.rebuiltcmdv3.constants.IntakeConstants; +import org.wpilib.examples.rebuiltcmdv3.stubs.ExampleSmartMotorController; import org.wpilib.units.measure.Angle; @Logged diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Shooter.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.java similarity index 95% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Shooter.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.java index 1e6cfde4597..33e06d6692b 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/Shooter.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/Shooter.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.command3.mechanisms; +package org.wpilib.examples.rebuiltcmdv3.mechanisms; import static org.wpilib.units.Units.Degrees; import static org.wpilib.units.Units.Meters; @@ -13,9 +13,9 @@ import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.constants.ShooterConstants; -import org.wpilib.examples.command3.lookup.LookupTables; -import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +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; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.java similarity index 97% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.java index 851cfaaf593..4571ae593bc 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveDrive.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveDrive.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.command3.mechanisms; +package org.wpilib.examples.rebuiltcmdv3.mechanisms; import static org.wpilib.units.Units.MetersPerSecond; import static org.wpilib.units.Units.Radians; @@ -14,9 +14,9 @@ import org.wpilib.command3.Mechanism; import org.wpilib.command3.button.CommandGamepad; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.constants.DriveConstants; -import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; -import org.wpilib.examples.command3.stubs.PathFollower; +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; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveModule.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java similarity index 94% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveModule.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java index 1c12732fc5d..59afc7cdc93 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/mechanisms/SwerveModule.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/mechanisms/SwerveModule.java @@ -2,10 +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.command3.mechanisms; +package org.wpilib.examples.rebuiltcmdv3.mechanisms; import org.wpilib.epilogue.Logged; -import org.wpilib.examples.command3.stubs.ExampleSmartMotorController; +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; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/DoNothingAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java similarity index 87% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/DoNothingAuto.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java index b2aab508314..4aabced4d1e 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/DoNothingAuto.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/DoNothingAuto.java @@ -2,9 +2,9 @@ // 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.command3.opmodes.auto; +package org.wpilib.examples.rebuiltcmdv3.opmodes.auto; -import org.wpilib.examples.command3.Robot; +import org.wpilib.examples.rebuiltcmdv3.Robot; import org.wpilib.opmode.Autonomous; import org.wpilib.opmode.OpMode; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/SweepAuto.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java similarity index 92% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/SweepAuto.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java index eecf826d105..ea282eaddd1 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/auto/SweepAuto.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/auto/SweepAuto.java @@ -2,13 +2,13 @@ // 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.command3.opmodes.auto; +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.command3.Robot; -import org.wpilib.examples.command3.constants.FieldConstants; +import org.wpilib.examples.rebuiltcmdv3.Robot; +import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants; import org.wpilib.opmode.Autonomous; import org.wpilib.opmode.OpMode; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java similarity index 89% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java index 2db5237e715..ec392156f6c 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/opmodes/teleop/DefaultTeleop.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/opmodes/teleop/DefaultTeleop.java @@ -2,10 +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.command3.opmodes.teleop; +package org.wpilib.examples.rebuiltcmdv3.opmodes.teleop; -import org.wpilib.examples.command3.Robot; -import org.wpilib.examples.command3.constants.FieldConstants; +import org.wpilib.examples.rebuiltcmdv3.Robot; +import org.wpilib.examples.rebuiltcmdv3.constants.FieldConstants; import org.wpilib.opmode.OpMode; import org.wpilib.opmode.Teleop; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/ExampleSmartMotorController.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.java similarity index 97% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/ExampleSmartMotorController.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.java index 63b2824ac9c..8d9518d7148 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/ExampleSmartMotorController.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/ExampleSmartMotorController.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.command3.stubs; +package org.wpilib.examples.rebuiltcmdv3.stubs; /** * A simplified stub class that simulates the API of a common "smart" motor controller. diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/PathFollower.java b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.java similarity index 96% rename from wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/PathFollower.java rename to wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.java index 046a6f498c9..b0109674ae9 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/command3/stubs/PathFollower.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/rebuiltcmdv3/stubs/PathFollower.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.command3.stubs; +package org.wpilib.examples.rebuiltcmdv3.stubs; import org.wpilib.math.kinematics.ChassisVelocities; From 4a2bb28556d0aac66f66f2eea7985ff8f15a5912 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Tue, 19 May 2026 22:30:34 -0400 Subject: [PATCH 08/13] Replace usages of NI PS5 controller with generic gamepad --- .../src/main/java/org/wpilib/templates/commandv3/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java index 48ff30e6219..7ff679f08c0 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/Robot.java @@ -5,15 +5,15 @@ package org.wpilib.templates.commandv3; import org.wpilib.command3.Scheduler; -import org.wpilib.command3.button.CommandNiDsPS5Controller; +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 CommandNiDsPS5Controller exampleController = - new CommandNiDsPS5Controller(DriverConstants.DRIVER_CONTROLLER_PORT); + final CommandGamepad exampleController = + new CommandGamepad(DriverConstants.DRIVER_CONTROLLER_PORT); public Robot() {} From af9155a9fae4bc89448ca5acaf5af3cb9aad652e Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Thu, 4 Jun 2026 22:02:52 -0400 Subject: [PATCH 09/13] Use new trigger name --- .../main/java/org/wpilib/templates/commandv3/ExampleTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java index 2b736c38a85..5d9defbcd52 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3/ExampleTeleop.java @@ -10,6 +10,6 @@ @Teleop public class ExampleTeleop implements OpMode { public ExampleTeleop(Robot robot) { - robot.exampleController.R1().whileTrue(robot.exampleMechanism.exampleCommand()); + robot.exampleController.rightStick().whileTrue(robot.exampleMechanism.exampleCommand()); } } From 09f73b9a986a686b0a17a4a770796c18e469bfad Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Thu, 4 Jun 2026 22:03:25 -0400 Subject: [PATCH 10/13] Remove Java-only template folders --- wpilibcExamples/example_projects.bzl | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index f66b0e9e5c6..e3255df765d 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -85,8 +85,6 @@ SNIPPET_FOLDERS = [ TEMPLATE_FOLDERS = [ "commandv2", "commandv2skeleton", - "commandv3", - "commandv3skeleton", "opmode", "robotbaseskeleton", "timed", From a088c2416b32848d20d96855a76c3cc8d6478460 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Thu, 4 Jun 2026 22:05:51 -0400 Subject: [PATCH 11/13] Linting --- .../main/java/org/wpilib/templates/commandv3skeleton/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java index e94385ab446..f484a0f7868 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/commandv3skeleton/Robot.java @@ -16,6 +16,6 @@ public Robot() {} @Override public void robotPeriodic() { Scheduler.getDefault().run(); - Epilogue.update(this); + Epilogue.update(this); } } From 43a83126810b1d46b7b5623f9bd1f673ce5342c7 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Thu, 4 Jun 2026 22:10:00 -0400 Subject: [PATCH 12/13] Use correct example and template folders --- wpilibcExamples/example_projects.bzl | 1 - wpilibjExamples/example_projects.bzl | 3 +++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index e3255df765d..af0bbb3520f 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -1,7 +1,6 @@ EXAMPLE_FOLDERS = [ "ArcadeDriveGamepad", "ArmSimulation", - "Commandv3", "DifferentialDriveBot", "DifferentialDrivePoseEstimator", "DriveDistanceOffboard", 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", From 7923c348b2d3bd7491253d6484f3e68b88e60ae5 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Sat, 13 Jun 2026 12:52:50 -0400 Subject: [PATCH 13/13] Update wpilbjExample bazel build Add missing commandv3 dependencies (needed for v3 examples) Add missing epilogue dependencies (needed for epilogue in examples/templates/snippets) Add missing wpiunits dependencies (was a transitive dependency of wpimath, which bazel disallowed) --- wpilibjExamples/build_java_examples.bzl | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) 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"], )