Change to YALL

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

@@ -4,9 +4,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.HootAutoReplay;
-
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -16,41 +13,13 @@ public class Robot extends TimedRobot {
 
     private final RobotContainer m_robotContainer;
 
-    /* log and replay timestamp and joystick data */
-    private final HootAutoReplay m_timeAndJoystickReplay = new HootAutoReplay()
-        .withTimestampReplay()
-        .withJoystickReplay();
-
-    private final boolean kUseLimelight = false;
-
     public Robot() {
         m_robotContainer = new RobotContainer();
     }
 
     @Override
     public void robotPeriodic() {
-        m_timeAndJoystickReplay.update();
         CommandScheduler.getInstance().run();
-
-        /*
-         * This example of adding Limelight is very simple and may not be sufficient for on-field use.
-         * Users typically need to provide a standard deviation that scales with the distance to target
-         * and changes with number of tags available.
-         *
-         * This example is sufficient to show that vision integration is possible, though exact implementation
-         * of how to use vision should be tuned per-robot and to the team's specification.
-         */
-        if (kUseLimelight) {
-            var driveState = m_robotContainer.drivetrain.getState();
-            double headingDeg = driveState.Pose.getRotation().getDegrees();
-            double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond);
-
-            LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0);
-            var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
-            if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
-                m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
-            }
-        }
     }
 
     @Override

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

@@ -4,11 +4,12 @@
 
 package frc.robot;
 
-import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.commands.FollowPathCommand;
 
@@ -19,13 +20,15 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.AprilTagsLimelight;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
 public class RobotContainer {
-    private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
+    private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top
-    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
+                                                                                        // speed
+    private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second
+                                                                                      // max angular velocity
 
     /* Setting up bindings for necessary control of the swerve drive platform */
     private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
@@ -41,6 +44,7 @@ public class RobotContainer {
     private final CommandXboxController joystick = new CommandXboxController(0);
 
     public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+    public final AprilTagsLimelight aprilTagsLimelight = new AprilTagsLimelight(drivetrain);
 
     /* Path follower */
     private final SendableChooser<Command> autoChooser;
@@ -59,32 +63,24 @@ public class RobotContainer {
         // Note that X is defined as forward according to WPILib convention,
         // and Y is defined as to the left according to WPILib convention.
         drivetrain.setDefaultCommand(
-            // Drivetrain will execute this command periodically
+                // Drivetrain will execute this command periodically
-            drivetrain.applyRequest(() ->
+                drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed)
-                drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward)
+                        .withVelocityY(-joystick.getLeftX() * MaxSpeed)
-                    .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
+                        .withRotationalRate(-joystick.getRightX() * MaxAngularRate)));
-                    .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left)
-            )
-        );
 
         // Idle while the robot is disabled. This ensures the configured
         // neutral mode is applied to the drive motors while disabled.
         final var idle = new SwerveRequest.Idle();
         RobotModeTriggers.disabled().whileTrue(
-            drivetrain.applyRequest(() -> idle).ignoringDisable(true)
+                drivetrain.applyRequest(() -> idle).ignoringDisable(true));
-        );
 
         joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
-        joystick.b().whileTrue(drivetrain.applyRequest(() ->
+        joystick.b().whileTrue(drivetrain.applyRequest(
-            point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))
+                () -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
-        ));
 
-        joystick.povUp().whileTrue(drivetrain.applyRequest(() ->
+        joystick.povUp().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
-            forwardStraight.withVelocityX(0.5).withVelocityY(0))
+        joystick.povDown()
-        );
+                .whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
-        joystick.povDown().whileTrue(drivetrain.applyRequest(() ->
-            forwardStraight.withVelocityX(-0.5).withVelocityY(0))
-        );
 
         // Run SysId routines when holding back/start and X/Y.
         // Note that each routine should be run exactly once in a single log.

src/main/java/frc/robot/Vision.java

@@ -0,0 +1,7 @@
+package frc.robot;
+
+import limelight.Limelight;
+
+public class Vision {
+    public final Limelight limelight = new Limelight("AprilTags");
+}

src/main/java/frc/robot/subsystems/AprilTagsLimelight.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems;
+
+import java.util.Optional;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import limelight.Limelight;
+import limelight.networktables.LimelightPoseEstimator;
+import limelight.networktables.LimelightPoseEstimator.EstimationMode;
+import limelight.networktables.PoseEstimate;
+
+public class AprilTagsLimelight extends SubsystemBase {
+
+    private CommandSwerveDrivetrain drivetrain;
+    private final Limelight limelight = new Limelight("AprilTags");
+    private final LimelightPoseEstimator poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2);;
+
+    public AprilTagsLimelight(CommandSwerveDrivetrain drivetrain) {
+        this.drivetrain = drivetrain;
+    }
+
+    @Override
+    public void periodic() {
+        limelight.getSettings()
+                .withRobotOrientation(drivetrain.getOrientation3d())
+                .save();
+
+        Optional<PoseEstimate> visionEstimate = poseEstimator.getPoseEstimate();
+        visionEstimate.ifPresent((PoseEstimate poseEstimate) -> {
+            drivetrain.addVisionMeasurement(poseEstimate.pose.toPose2d(), poseEstimate.timestampSeconds);
+        });
+    }
+
+}

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
-import static edu.wpi.first.units.Units.*;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Volts;
 
 import java.util.Optional;
 import java.util.function.Supplier;
@@ -10,7 +11,6 @@ import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveRequest;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
@@ -29,8 +29,8 @@ import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+import limelight.networktables.Orientation3d;
 
 /**
  * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
@@ -59,64 +59,65 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
     private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
 
-    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+    /*
+     * SysId routine for characterizing translation. This is used to find PID gains
+     * for the drive motors.
+     */
     private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
-        new SysIdRoutine.Config(
+            new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
+                    null, // Use default ramp rate (1 V/s)
-            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
+                    Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
-            null,        // Use default timeout (10 s)
+                    null, // Use default timeout (10 s)
-            // Log state with SignalLogger class
+                    // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
+                    state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())),
-        ),
+            new SysIdRoutine.Mechanism(
-        new SysIdRoutine.Mechanism(
+                    output -> setControl(m_translationCharacterization.withVolts(output)),
-            output -> setControl(m_translationCharacterization.withVolts(output)),
+                    null,
-            null,
+                    this));
-            this
-        )
-    );
 
-    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
+    /*
+     * SysId routine for characterizing steer. This is used to find PID gains for
+     * the steer motors.
+     */
+    @SuppressWarnings("unused")
     private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
-        new SysIdRoutine.Config(
+            new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
+                    null, // Use default ramp rate (1 V/s)
-            Volts.of(7), // Use dynamic voltage of 7 V
+                    Volts.of(7), // Use dynamic voltage of 7 V
-            null,        // Use default timeout (10 s)
+                    null, // Use default timeout (10 s)
-            // Log state with SignalLogger class
+                    // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
+                    state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
-        ),
+            new SysIdRoutine.Mechanism(
-        new SysIdRoutine.Mechanism(
+                    volts -> setControl(m_steerCharacterization.withVolts(volts)),
-            volts -> setControl(m_steerCharacterization.withVolts(volts)),
+                    null,
-            null,
+                    this));
-            this
-        )
-    );
 
     /*
      * SysId routine for characterizing rotation.
-     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
+     * This is used to find PID gains for the FieldCentricFacingAngle
-     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
+     * HeadingController.
+     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on
+     * importing the log to SysId.
      */
+    @SuppressWarnings("unused")
     private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
-        new SysIdRoutine.Config(
+            new SysIdRoutine.Config(
-            /* This is in radians per second², but SysId only supports "volts per second" */
+                    /* This is in radians per second², but SysId only supports "volts per second" */
-            Volts.of(Math.PI / 6).per(Second),
+                    Volts.of(Math.PI / 6).per(Second),
-            /* This is in radians per second, but SysId only supports "volts" */
+                    /* This is in radians per second, but SysId only supports "volts" */
-            Volts.of(Math.PI),
+                    Volts.of(Math.PI),
-            null, // Use default timeout (10 s)
+                    null, // Use default timeout (10 s)
-            // Log state with SignalLogger class
+                    // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
+                    state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
-        ),
+            new SysIdRoutine.Mechanism(
-        new SysIdRoutine.Mechanism(
+                    output -> {
-            output -> {
+                        /* output is actually radians per second, but SysId only supports "volts" */
-                /* output is actually radians per second, but SysId only supports "volts" */
+                        setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
-                setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
+                        /* also log the requested output for SysId */
-                /* also log the requested output for SysId */
+                        SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
-                SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
+                    },
-            },
+                    null,
-            null,
+                    this));
-            this
-        )
-    );
 
     /* The SysId routine to test */
     private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
@@ -124,17 +125,18 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
-     * This constructs the underlying hardware devices, so users should not construct
+     * This constructs the underlying hardware devices, so users should not
-     * the devices themselves. If they need the devices, they can access them through
+     * construct
+     * the devices themselves. If they need the devices, they can access them
+     * through
      * getters in the classes.
      *
-     * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+     * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
-     * @param modules               Constants for each specific module
+     * @param modules             Constants for each specific module
      */
     public CommandSwerveDrivetrain(
-        SwerveDrivetrainConstants drivetrainConstants,
+            SwerveDrivetrainConstants drivetrainConstants,
-        SwerveModuleConstants<?, ?, ?>... modules
+            SwerveModuleConstants<?, ?, ?>... modules) {
-    ) {
         super(drivetrainConstants, modules);
         if (Utils.isSimulation()) {
             startSimThread();
@@ -145,8 +147,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
-     * This constructs the underlying hardware devices, so users should not construct
+     * This constructs the underlying hardware devices, so users should not
-     * the devices themselves. If they need the devices, they can access them through
+     * construct
+     * the devices themselves. If they need the devices, they can access them
+     * through
      * getters in the classes.
      *
      * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
@@ -156,10 +160,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
      * @param modules                 Constants for each specific module
      */
     public CommandSwerveDrivetrain(
-        SwerveDrivetrainConstants drivetrainConstants,
+            SwerveDrivetrainConstants drivetrainConstants,
-        double odometryUpdateFrequency,
+            double odometryUpdateFrequency,
-        SwerveModuleConstants<?, ?, ?>... modules
+            SwerveModuleConstants<?, ?, ?>... modules) {
-    ) {
         super(drivetrainConstants, odometryUpdateFrequency, modules);
         if (Utils.isSimulation()) {
             startSimThread();
@@ -170,30 +173,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
-     * This constructs the underlying hardware devices, so users should not construct
+     * This constructs the underlying hardware devices, so users should not
-     * the devices themselves. If they need the devices, they can access them through
+     * construct
+     * the devices themselves. If they need the devices, they can access them
+     * through
      * getters in the classes.
      *
-     * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+     * @param drivetrainConstants       Drivetrain-wide constants for the swerve
+     *                                  drive
      * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
-     *                                  unspecified or set to 0 Hz, this is 250 Hz on
+     *                                  unspecified or set to 0 Hz, this is 250 Hz
+     *                                  on
      *                                  CAN FD, and 100 Hz on CAN 2.0.
-     * @param odometryStandardDeviation The standard deviation for odometry calculation
+     * @param odometryStandardDeviation The standard deviation for odometry
-     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in
+     *                                  meters
      *                                  and radians
-     * @param visionStandardDeviation   The standard deviation for vision calculation
+     * @param visionStandardDeviation   The standard deviation for vision
-     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in
+     *                                  meters
      *                                  and radians
      * @param modules                   Constants for each specific module
      */
     public CommandSwerveDrivetrain(
-        SwerveDrivetrainConstants drivetrainConstants,
+            SwerveDrivetrainConstants drivetrainConstants,
-        double odometryUpdateFrequency,
+            double odometryUpdateFrequency,
-        Matrix<N3, N1> odometryStandardDeviation,
+            Matrix<N3, N1> odometryStandardDeviation,
-        Matrix<N3, N1> visionStandardDeviation,
+            Matrix<N3, N1> visionStandardDeviation,
-        SwerveModuleConstants<?, ?, ?>... modules
+            SwerveModuleConstants<?, ?, ?>... modules) {
-    ) {
+        super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation,
-        super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
+                modules);
         if (Utils.isSimulation()) {
             startSimThread();
         }
@@ -204,33 +215,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         try {
             var config = RobotConfig.fromGUISettings();
             AutoBuilder.configure(
-                () -> getState().Pose,   // Supplier of current robot pose
+                    () -> getState().Pose, // Supplier of current robot pose
-                this::resetPose,         // Consumer for seeding pose against auto
+                    this::resetPose, // Consumer for seeding pose against auto
-                () -> getState().Speeds, // Supplier of current robot speeds
+                    () -> getState().Speeds, // Supplier of current robot speeds
-                // Consumer of ChassisSpeeds and feedforwards to drive the robot
+                    // Consumer of ChassisSpeeds and feedforwards to drive the robot
-                (speeds, feedforwards) -> setControl(
+                    (speeds, feedforwards) -> setControl(
-                    m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
+                            m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
-                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
+                                    .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
-                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
+                                    .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())),
-                ),
+                    new PPHolonomicDriveController(
-                new PPHolonomicDriveController(
+                            // PID constants for translation
-                    // PID constants for translation
+                            new PIDConstants(10, 0, 0),
-                    new PIDConstants(10, 0, 0),
+                            // PID constants for rotation
-                    // PID constants for rotation
+                            new PIDConstants(7, 0, 0)),
-                    new PIDConstants(7, 0, 0)
+                    config,
-                ),
+                    // Assume the path needs to be flipped for Red vs Blue, this is normally the
-                config,
+                    // case
-                // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+                    () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
-                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                    this // Subsystem for requirements
-                this // Subsystem for requirements
             );
         } catch (Exception ex) {
-            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder",
+                    ex.getStackTrace());
         }
     }
 
     /**
-     * Returns a command that applies the specified control request to this swerve drivetrain.
+     * Returns a command that applies the specified control request to this swerve
+     * drivetrain.
      *
      * @param request Function returning the request to apply
      * @return Command to run
@@ -261,22 +273,32 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         return m_sysIdRoutineToApply.dynamic(direction);
     }
 
+    public Orientation3d getOrientation3d() {
+        return new Orientation3d(getRotation3d(),
+                getPigeon2().getAngularVelocityZDevice().getValue(), // Yaw
+                getPigeon2().getAngularVelocityYDevice().getValue(), // Pitch
+                getPigeon2().getAngularVelocityXDevice().getValue()); // Roll
+    }
+
     @Override
     public void periodic() {
         /*
          * Periodically try to apply the operator perspective.
-         * If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
+         * If we haven't applied the operator perspective before, then we should apply
-         * This allows us to correct the perspective in case the robot code restarts mid-match.
+         * it regardless of DS state.
-         * Otherwise, only check and apply the operator perspective if the DS is disabled.
+         * This allows us to correct the perspective in case the robot code restarts
-         * This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
+         * mid-match.
+         * Otherwise, only check and apply the operator perspective if the DS is
+         * disabled.
+         * This ensures driving behavior doesn't change until an explicit disable event
+         * occurs during testing.
          */
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {
                 setOperatorPerspectiveForward(
-                    allianceColor == Alliance.Red
+                        allianceColor == Alliance.Red
-                        ? kRedAlliancePerspectiveRotation
+                                ? kRedAlliancePerspectiveRotation
-                        : kBlueAlliancePerspectiveRotation
+                                : kBlueAlliancePerspectiveRotation);
-                );
                 m_hasAppliedOperatorPerspective = true;
             });
         }
@@ -298,11 +320,14 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * Adds a vision measurement to the Kalman Filter. This will correct the
+     * odometry pose estimate
      * while still accounting for measurement noise.
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     *                              camera.
+     * @param timestampSeconds      The timestamp of the vision measurement in
+     *                              seconds.
      */
     @Override
     public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -310,32 +335,38 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * Adds a vision measurement to the Kalman Filter. This will correct the
+     * odometry pose estimate
      * while still accounting for measurement noise.
      * <p>
      * Note that the vision measurement standard deviations passed into this method
      * will continue to apply to future measurements until a subsequent call to
      * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param visionRobotPoseMeters    The pose of the robot as measured by the
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     *                                 vision camera.
-     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     * @param timestampSeconds         The timestamp of the vision measurement in
-     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     *                                 seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose
+     *                                 measurement
+     *                                 in the form [x, y, theta]ᵀ, with units in
+     *                                 meters and radians.
      */
     @Override
     public void addVisionMeasurement(
-        Pose2d visionRobotPoseMeters,
+            Pose2d visionRobotPoseMeters,
-        double timestampSeconds,
+            double timestampSeconds,
-        Matrix<N3, N1> visionMeasurementStdDevs
+            Matrix<N3, N1> visionMeasurementStdDevs) {
-    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds),
-        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+                visionMeasurementStdDevs);
     }
 
     /**
      * Return the pose at a given timestamp, if the buffer is not empty.
      *
      * @param timestampSeconds The timestamp of the pose in seconds.
-     * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+     * @return The pose at the given timestamp (or Optional.empty() if the buffer is
+     *         empty).
      */
     @Override
     public Optional<Pose2d> samplePoseAt(double timestampSeconds) {

vendordeps/yall.json

@@ -0,0 +1,20 @@
+{
+    "fileName": "yall.json",
+    "name": "YALL",
+    "version": "2026.1.13",
+    "frcYear": "2026",
+    "uuid": "29aa8c8f-05ae-40aa-96f9-552a5c8a0347",
+    "mavenUrls": [
+        "https://Yet-Another-Software-Suite.github.io/YALL/releases"
+    ],
+    "jsonUrl": "https://Yet-Another-Software-Suite.github.io/YALL/yall.json",
+    "javaDependencies": [
+        {
+            "groupId": "yall",
+            "artifactId": "YALL-java",
+            "version": "2026.1.13"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}