Update WPILib, vendordeps; make conformal changes

.vscode/settings.json

@@ -36,6 +36,36 @@
   "spotlessGradle.format.enable": true,
   "spotlessGradle.diagnostics.enable": false,
   "java.import.gradle.annotationProcessing.enabled": false,
+  "java.completion.favoriteStaticMembers": [
+    "org.junit.Assert.*",
+    "org.junit.Assume.*",
+    "org.junit.jupiter.api.Assertions.*",
+    "org.junit.jupiter.api.Assumptions.*",
+    "org.junit.jupiter.api.DynamicContainer.*",
+    "org.junit.jupiter.api.DynamicTest.*",
+    "org.mockito.Mockito.*",
+    "org.mockito.ArgumentMatchers.*",
+    "org.mockito.Answers.*",
+    "edu.wpi.first.units.Units.*"
+  ],
+  "java.completion.filteredTypes": [
+    "java.awt.*",
+    "com.sun.*",
+    "sun.*",
+    "jdk.*",
+    "org.graalvm.*",
+    "io.micrometer.shaded.*",
+    "java.beans.*",
+    "java.util.Base64.*",
+    "java.util.Timer",
+    "java.sql.*",
+    "javax.swing.*",
+    "javax.management.*",
+    "javax.smartcardio.*",
+    "edu.wpi.first.math.proto.*",
+    "edu.wpi.first.math.**.proto.*",
+    "edu.wpi.first.math.**.struct.*"
+  ],
   "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle",
   "[json]": {
     "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
     id "com.peterabeles.gversion" version "1.10.3"
     id "com.diffplug.spotless" version "6.25.0"
     id "io.freefair.lombok" version "8.11"

src/main/java/frc/robot/Constants.java

@@ -238,7 +238,7 @@ public static class AprilTagConstants {
     public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL;
 
     public static final AprilTagFieldLayout aprilTagFieldLayout =
-        AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+        AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
     @Getter
     public enum AprilTagLayoutType {

src/main/java/frc/robot/Robot.java

@@ -13,7 +13,6 @@
 
 package frc.robot;
 
-import choreo.auto.AutoRoutine;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -34,7 +33,6 @@
  */
 public class Robot extends LoggedRobot {
   private Command m_autoCommandPathPlanner;
-  private AutoRoutine m_autoCommandChoreo;
   private RobotContainer m_robotContainer;
   private Timer m_disabledTimer;
 
@@ -155,11 +153,7 @@ public void autonomousInit() {
         }
         break;
       case CHOREO:
-        m_autoCommandChoreo = m_robotContainer.getAutonomousCommandChoreo();
-        // schedule the autonomous command (example)
-        if (m_autoCommandChoreo != null) {
-          CommandScheduler.getInstance().schedule(m_autoCommandChoreo.cmd());
-        }
+        m_robotContainer.getAutonomousCommandChoreo();
         break;
       default:
         throw new RuntimeException(

src/main/java/frc/robot/RobotContainer.java

@@ -21,30 +21,27 @@
 
 import static frc.robot.subsystems.vision.VisionConstants.*;
 
-import choreo.Choreo;
 import choreo.auto.AutoChooser;
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoFactory.AutoBindings;
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.AprilTagConstants;
 import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType;
-import frc.robot.commands.ChoreoAutoController;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.accelerometer.Accelerometer;
 import frc.robot.subsystems.drive.Drive;
@@ -91,7 +88,6 @@ public class RobotContainer {
   private final LoggedDashboardChooser<Command> autoChooserPathPlanner;
   private final AutoChooser autoChooserChoreo;
   private final AutoFactory autoFactoryChoreo;
-  private final ChoreoAutoController choreoController;
   // Input estimated battery capacity (if full, use printed value)
   private final LoggedTunableNumber batteryCapacity =
       new LoggedTunableNumber("Battery Amp-Hours", 18.0);
@@ -176,21 +172,20 @@ public RobotContainer() {
         // Set the others to null
         autoChooserChoreo = null;
         autoFactoryChoreo = null;
-        choreoController = null;
         break;
       case CHOREO:
-        choreoController = new ChoreoAutoController(m_drivebase);
         autoFactoryChoreo =
-            Choreo.createAutoFactory(
+            new AutoFactory(
                 m_drivebase::getPose, // A function that returns the current robot pose
-                choreoController, // The controller for the drive subsystem
-                this::isRedAlliance, // A function that returns true if the robot is on the red
-                // alliance
-                m_drivebase,
-                new AutoBindings() // An empty `AutoBindings` object, you can learn more below
+                m_drivebase::resetOdometry, // A function that resets the current robot pose to the
+                // provided Pose2d
+                m_drivebase::followTrajectory, // The drive subsystem trajectory follower
+                true, // If alliance flipping should be enabled
+                m_drivebase, // The drive subsystem
+                new AutoBindings() // An empty AutoBindings object
                 );
-        autoChooserChoreo = new AutoChooser(autoFactoryChoreo, "");
-        autoChooserChoreo.addAutoRoutine("twoPieceAuto", this::twoPieceAuto);
+        autoChooserChoreo = new AutoChooser();
+        autoChooserChoreo.addRoutine("twoPieceAuto", this::twoPieceAuto);
         // Set the others to null
         autoChooserPathPlanner = null;
         break;
@@ -322,8 +317,12 @@ public Command getAutonomousCommandPathPlanner() {
    *
    * @return the command to run in autonomous
    */
-  public AutoRoutine getAutonomousCommandChoreo() {
-    return autoChooserChoreo.getSelectedAutoRoutine();
+  public void getAutonomousCommandChoreo() {
+    // Put the auto chooser on the dashboard
+    SmartDashboard.putData(autoChooserChoreo);
+
+    // Schedule the selected auto during the autonomous period
+    RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler());
   }
 
   public void setDriveMode() {
@@ -435,33 +434,28 @@ public static class Cameras {
    *
    * <p>NOTE: This would normally be in a spearate file.
    */
-  private AutoRoutine twoPieceAuto(AutoFactory factory) {
-    final AutoRoutine routine = factory.newRoutine("twoPieceAuto");
+  private AutoRoutine twoPieceAuto() {
+    AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto");
 
-    final AutoTrajectory trajectory = routine.trajectory("twoPieceAuto");
+    // Load the routine's trajectories
+    AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece");
+    AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece");
 
-    routine
-        .running()
-        .onTrue(
-            m_drivebase
-                .resetOdometry(
-                    trajectory
-                        .getInitialPose()
-                        .orElseGet(
-                            () -> {
-                              routine.kill();
-                              return new Pose2d();
-                            }))
-                .andThen(trajectory.cmd())
-                .withName("twoPieceAuto entry point"));
-
-    // trajectory.atTime("intake").onTrue(intake.extend());
-    // trajectory.atTime("shoot").onTrue(shooter.launch());
+    // When the routine begins, reset odometry and start the first trajectory
+    routine.active().onTrue(Commands.sequence(routine.resetOdometry(pickupTraj), pickupTraj.cmd()));
 
-    return routine;
-  }
+    // Starting at the event marker named "intake", run the intake
+    // pickupTraj.atTime("intake").onTrue(intakeSubsystem.intake());
 
-  private boolean isRedAlliance() {
-    return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Red);
+    // When the trajectory is done, start the next trajectory
+    pickupTraj.done().onTrue(scoreTraj.cmd());
+
+    // While the trajectory is active, prepare the scoring subsystem
+    // scoreTraj.active().whileTrue(scoringSubsystem.getReady());
+
+    // When the trajectory is done, score
+    // scoreTraj.done().onTrue(scoringSubsystem.score());
+
+    return routine;
   }
 }

src/main/java/frc/robot/commands/ChoreoAutoController.java

@@ -50,10 +50,10 @@ public void accept(SwerveSample referenceState) {
     double rotationFeedback =
         headingController.calculate(pose.getRotation().getRadians(), referenceState.heading);
 
+    // Convert to field relative speeds & send command
     ChassisSpeeds out =
         ChassisSpeeds.fromFieldRelativeSpeeds(
             xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation());
-
     drive.runVelocity(out);
   }
 }

src/main/java/frc/robot/commands/DriveCommands.java

@@ -97,18 +97,12 @@ public static Command robotRelativeDrive(
           Translation2d linearVelocity = getLinearVelocity(xSupplier, ySupplier);
           double omega = getOmega(omegaSupplier);
 
-          // Convert to robot relative speeds & send command
-          boolean isFlipped =
-              DriverStation.getAlliance().isPresent()
-                  && DriverStation.getAlliance().get() == Alliance.Red;
+          // Run with straight-up velocities w.r.t. the robot!
           drive.runVelocity(
-              ChassisSpeeds.fromRobotRelativeSpeeds(
+              new ChassisSpeeds(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
-                  omega * drive.getMaxAngularSpeedRadPerSec(),
-                  isFlipped
-                      ? drive.getRotation().plus(new Rotation2d(Math.PI))
-                      : drive.getRotation()));
+                  omega * drive.getMaxAngularSpeedRadPerSec()));
         },
         drive);
   }
@@ -188,9 +182,12 @@ public static Command joystickDrive(
           boolean isFlipped =
               DriverStation.getAlliance().isPresent()
                   && DriverStation.getAlliance().get() == Alliance.Red;
-          speeds.toRobotRelativeSpeeds(
-              isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation());
-          drive.runVelocity(speeds);
+          drive.runVelocity(
+              ChassisSpeeds.fromFieldRelativeSpeeds(
+                  speeds,
+                  isFlipped
+                      ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                      : drive.getRotation()));
         },
         drive);
   }
@@ -236,11 +233,12 @@ public static Command joystickDriveAtAngle(
               boolean isFlipped =
                   DriverStation.getAlliance().isPresent()
                       && DriverStation.getAlliance().get() == Alliance.Red;
-              speeds.toRobotRelativeSpeeds(
-                  isFlipped
-                      ? drive.getRotation().plus(new Rotation2d(Math.PI))
-                      : drive.getRotation());
-              drive.runVelocity(speeds);
+              drive.runVelocity(
+                  ChassisSpeeds.fromFieldRelativeSpeeds(
+                      speeds,
+                      isFlipped
+                          ? drive.getRotation().plus(new Rotation2d(Math.PI))
+                          : drive.getRotation()));
             },
             drive)
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -105,6 +105,9 @@ public class Drive extends SubsystemBase {
   private SwerveDrivePoseEstimator m_PoseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
 
+  // Choreo drive controller
+  private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
+
   // Constructor
   public Drive() {
     switch (Constants.getSwerveType()) {
@@ -282,13 +285,13 @@ public void setMotorBrake(boolean brake) {
    */
   public void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
-    speeds.discretize(Constants.loopPeriodSecs);
-    SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds);
+    ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
+    SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
     SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed);
 
     // Log unoptimized setpoints and setpoint speeds
     Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
-    Logger.recordOutput("SwerveChassisSpeeds/Setpoints", speeds);
+    Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds);
 
     // Send setpoints to modules
     for (int i = 0; i < 4; i++) {
@@ -468,6 +471,21 @@ public void choreoController(Pose2d pose, SwerveSample sample) {
     //         .withWheelForceFeedforwardsY(sample.moduleForcesY()));
   }
 
+  public void followTrajectory(SwerveSample sample) {
+    // Get the current pose of the robot
+    Pose2d pose = getPose();
+
+    // Generate the next speeds for the robot
+    ChassisSpeeds speeds =
+        new ChassisSpeeds(
+            sample.vx + xController.calculate(pose.getX(), sample.x),
+            sample.vy + xController.calculate(pose.getX(), sample.y),
+            sample.omega + xController.calculate(pose.getRotation().getRadians(), sample.heading));
+
+    // Apply the generated speeds
+    runVelocity(speeds);
+  }
+
   /**
    * Parse the module type given the type information for the FL module
    *

src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java

@@ -80,38 +80,38 @@ public void updateInputs(VisionIOInputs inputs) {
     List<PoseObservation> poseObservations = new LinkedList<>();
     for (var rawSample : megatag1Subscriber.readQueue()) {
       if (rawSample.value.length == 0) continue;
-      for (int i = 10; i < rawSample.value.length; i += 7) {
+      for (int i = 11; i < rawSample.value.length; i += 7) {
         tagIds.add((int) rawSample.value[i]);
       }
       poseObservations.add(
           new PoseObservation(
               // Timestamp, based on server timestamp of publish and latency
-              rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3,
+              rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3,
 
               // 3D pose estimate
               parsePose(rawSample.value),
 
               // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag
-              rawSample.value.length >= 17 ? rawSample.value[16] : 0.0,
+              rawSample.value.length >= 18 ? rawSample.value[17] : 0.0,
 
               // Tag count
-              (int) rawSample.value[8],
+              (int) rawSample.value[7],
 
               // Average tag distance
-              rawSample.value[10],
+              rawSample.value[9],
 
               // Observation type
               PoseObservationType.MEGATAG_1));
     }
     for (var rawSample : megatag2Subscriber.readQueue()) {
       if (rawSample.value.length == 0) continue;
-      for (int i = 10; i < rawSample.value.length; i += 7) {
+      for (int i = 11; i < rawSample.value.length; i += 7) {
         tagIds.add((int) rawSample.value[i]);
       }
       poseObservations.add(
           new PoseObservation(
               // Timestamp, based on server timestamp of publish and latency
-              rawSample.timestamp * 1.0e-9 - rawSample.value[7] * 1.0e-3,
+              rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3,
 
               // 3D pose estimate
               parsePose(rawSample.value),
@@ -120,10 +120,10 @@ public void updateInputs(VisionIOInputs inputs) {
               0.0,
 
               // Tag count
-              (int) rawSample.value[8],
+              (int) rawSample.value[7],
 
               // Average tag distance
-              rawSample.value[10],
+              rawSample.value[9],
 
               // Observation type
               PoseObservationType.MEGATAG_2));