[cmd3] Add example and template projects for commands v3 and make hatchbot example idiomatic

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

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

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

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

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<Pose2d> targetHub() {
+    return DriverStationBackend.getAlliance()
+        .map(
+            a -> {
+              return switch (a) {
+                case RED -> RED_HUB_CENTER;
+                case BLUE -> BLUE_HUB_CENTER;
+              };
+            });
+  }
+}

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

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

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 <U> The type of the unit for the measure
+   * @param <M> The type of the measure
+   * @return The interpolator for the measure
+   */
+  @SuppressWarnings("unchecked")
+  public static <U extends Unit, M extends Measure<U>> Interpolator<M> 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 <U> The type of the unit for the measure
+   * @param <M> The type of the measure
+   * @return The inverse interpolator for the measure
+   */
+  public static <U extends Unit, M extends Measure<U>>
+      InverseInterpolator<M> inverseUnitInterpolator() {
+    return (startValue, endValue, q) -> {
+      Measure<U> totalRange = endValue.minus(startValue);
+      if (totalRange.baseUnitMagnitude() <= 0) {
+        return 0;
+      }
+
+      Measure<U> queryToStart = q.minus(startValue);
+      if (queryToStart.baseUnitMagnitude() <= 0) {
+        return 0;
+      }
+
+      return ((Dimensionless) queryToStart.div(totalRange)).in(Value);
+    };
+  }
+}