Add latest vision code

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

@@ -77,7 +77,7 @@ public class Robot extends LoggedRobot {
       new ConveyorSubsystem(
           new TalonFX(RobotConfig.get().conveyor().motorID(), "rio"),
           new DigitalInput(RobotConfig.get().conveyor().sensorID()));
-  private final VisionSubsystem vision = new VisionSubsystem();
+  private final VisionSubsystem vision = new VisionSubsystem(imu);
   private final LocalizationSubsystem localization = new LocalizationSubsystem(swerve, imu, vision);
   private final SnapManager snaps = new SnapManager(swerve, driverController);
   private final NoteManager noteManager = new NoteManager(queuer, intake, conveyor);

src/main/java/frc/robot/imu/ImuSubsystem.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import frc.robot.config.RobotConfig;
 import frc.robot.swerve.SwerveSubsystem;
+import frc.robot.util.TimedDataBuffer;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import org.littletonrobotics.junction.Logger;
@@ -19,6 +20,8 @@ public class ImuSubsystem extends LifecycleSubsystem {
   private final Pigeon2 imu;
   private final InterpolatingDoubleTreeMap distanceToAngleTolerance =
       new InterpolatingDoubleTreeMap();
+  private final TimedDataBuffer robotHeadingLatency = new TimedDataBuffer(6);
+  private final TimedDataBuffer robotAngularVelocityLatency = new TimedDataBuffer(6);
 
   public ImuSubsystem(SwerveSubsystem swerve) {
     super(SubsystemPriority.IMU);
@@ -48,10 +51,18 @@ public Rotation2d getRobotHeading() {
     return Rotation2d.fromDegrees(imu.getYaw().getValue());
   }
 
+  public Rotation2d getRobotHeading(double timestamp) {
+    return new Rotation2d().fromDegrees(robotHeadingLatency.lookupData(timestamp));
+  }
+
   public Rotation2d getRobotAngularVelocity() {
     return Rotation2d.fromDegrees(imu.getRate());
   }
 
+  public Rotation2d getRobotAngularVelocity(double timestamp) {
+    return Rotation2d.fromDegrees(robotAngularVelocityLatency.lookupData(timestamp));
+  }
+
   public void setAngle(Rotation2d zeroAngle) {
     this.imu.setYaw(zeroAngle.getDegrees());
   }

src/main/java/frc/robot/localization/LocalizationSubsystem.java

@@ -4,21 +4,23 @@
 
 package frc.robot.localization;
 
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.vision.FastLimelightResults;
 import frc.robot.vision.LimelightHelpers;
-import frc.robot.vision.PoseWithTagID;
 import frc.robot.vision.VisionSubsystem;
 import org.littletonrobotics.junction.Logger;
 
@@ -31,6 +33,7 @@ public class LocalizationSubsystem extends LifecycleSubsystem {
   private final SwerveDrivePoseEstimator poseEstimator;
   private final SwerveDriveOdometry odometry;
   private final VisionSubsystem vision;
+  private double lastAddedVisionTimestamp = 0;
 
   public LocalizationSubsystem(SwerveSubsystem swerve, ImuSubsystem imu, VisionSubsystem vision) {
     super(SubsystemPriority.LOCALIZATION);
@@ -57,17 +60,27 @@ public void robotPeriodic() {
     poseEstimator.update(
         imu.getRobotHeading(), swerve.getModulePositions().toArray(new SwerveModulePosition[4]));
 
-    try {
-      var results = vision.getFilteredAprilTags();
-      for (PoseWithTagID fiducial : results.tags()) {
-        poseEstimator.addVisionMeasurement(fiducial.robotPose().toPose2d(), results.latency());
+    FastLimelightResults results = VisionSubsystem.getFastResults();
+    Pose2d visionPose = results.robotPose().toPose2d();
+    double limelightStandardDeviation = vision.getStandardDeviation(results.distanceToTag());
+    if (vision.isResultValid(results)) {
+      double timestamp = Timer.getFPGATimestamp() - results.latency();
+
+      if (timestamp != lastAddedVisionTimestamp) {
+        // Don't add the same vision pose over and over
+        poseEstimator.addVisionMeasurement(
+            visionPose,
+            timestamp,
+            VecBuilder.fill(
+                limelightStandardDeviation,
+                limelightStandardDeviation,
+                limelightStandardDeviation));
+        lastAddedVisionTimestamp = timestamp;
       }
-    } catch (Exception e) {
     }
 
     Logger.recordOutput("Localization/OdometryPose", getOdometryPose());
     Logger.recordOutput("Localization/EstimatedPose", getPose());
-    Logger.recordOutput("Localization/ExpectedPose", getExpectedPose(SHOOT_WHILE_MOVE_LOOKAHEAD));
     Logger.recordOutput("Localization/AccelerationX", imu.getXAcceleration());
     Logger.recordOutput("Localization/AccelerationY", imu.getYAcceleration());
     Logger.recordOutput(
@@ -86,6 +99,7 @@ public Pose2d getPose() {
   }
 
   public Pose2d getOdometryPose() {
+
     return odometry.getPoseMeters();
   }
 
@@ -108,21 +122,28 @@ public Command getZeroCommand() {
   }
 
   public Pose2d getExpectedPose(double lookAhead) {
-    if (!USE_SHOOT_WHILE_MOVE) {
-      return getPose();
-    }
-
     var velocities = swerve.getRobotRelativeSpeeds();
     var angularVelocity = imu.getRobotAngularVelocity();
 
     var xDifference =
-        (velocities.vxMetersPerSecond + (imu.getXAcceleration() * lookAhead)) * lookAhead;
+        imu.getXAcceleration() * Math.pow(lookAhead, 2) / 2
+            + velocities.vxMetersPerSecond * lookAhead;
     var yDifference =
-        (velocities.vyMetersPerSecond + (imu.getYAcceleration() * lookAhead)) * lookAhead;
+        imu.getYAcceleration() * Math.pow(lookAhead, 2) / 2
+            + velocities.vyMetersPerSecond * lookAhead;
     var thetaDifference = new Rotation2d(angularVelocity.getRadians() * lookAhead);
 
-    return new Pose2d(
-        new Translation2d(xDifference + getPose().getX(), yDifference + getPose().getY()),
-        thetaDifference.plus(imu.getRobotHeading()));
+    var expectedPose =
+        new Pose2d(
+            new Translation2d(xDifference + getPose().getX(), yDifference + getPose().getY()),
+            thetaDifference.plus(imu.getRobotHeading()));
+
+    Logger.recordOutput("Localization/ExpectedPose", expectedPose);
+
+    if (!USE_SHOOT_WHILE_MOVE) {
+      return getPose();
+    }
+
+    return expectedPose;
   }
 }

src/main/java/frc/robot/robot_manager/RobotManager.java

@@ -77,7 +77,7 @@ public void robotPeriodic() {
     flags.log();
 
     DistanceAngle speakerVisionTargets = vision.getDistanceAngleSpeaker();
-    DistanceAngle floorSpotVisionTargets = vision.getDistanceAngleFloorSpot();
+    DistanceAngle floorSpotVisionTargets = vision.getDistanceAngleFloorShot();
     double speakerDistance = speakerVisionTargets.distance();
     double floorSpotDistance = floorSpotVisionTargets.distance();
     Rotation2d wristAngleForSpeaker = wrist.getAngleFromDistanceToSpeaker(speakerDistance);

src/main/java/frc/robot/util/TimeData.java

@@ -0,0 +1,7 @@
+// 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 frc.robot.util;
+
+public record TimeData(double time, double data) {}

src/main/java/frc/robot/util/TimedDataBuffer.java

@@ -0,0 +1,45 @@
+// 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 frc.robot.util;
+
+import java.util.ArrayList;
+
+public class TimedDataBuffer {
+  ArrayList<TimeData> dataSet = new ArrayList<TimeData>();
+  private int size = 0;
+
+  public TimedDataBuffer(int size) {
+    this.size = size;
+  }
+
+  public void addData(double time, double data) {
+    if (dataSet.size() > size) {
+      dataSet.remove(0);
+    }
+    dataSet.add(new TimeData(time, data));
+  }
+
+  public double lookupData(double time) {
+    if (time <= dataSet.get(0).time()) {
+      return dataSet.get(0).data();
+    }
+    if (time >= dataSet.get(dataSet.size() - 1).time()) {
+      return dataSet.get(dataSet.size() - 1).data();
+    }
+
+    for (int i = 0; i < dataSet.size() - 1; i++) {
+      double x1 = dataSet.get(i).time();
+      double x2 = dataSet.get(i + 1).time();
+      double y1 = dataSet.get(i).data();
+      double y2 = dataSet.get(i + 1).data();
+
+      if (time >= x1 && time <= x1) {
+        double slope = (y2 - y1) / (x2 - x1);
+        return (slope * time) + (y1 - (slope * x1));
+      }
+    }
+    return dataSet.get(dataSet.size() - 1).data();
+  }
+}

src/main/java/frc/robot/vision/FastLimelightResults.java

@@ -4,6 +4,7 @@
 
 package frc.robot.vision;
 
-import java.util.List;
+import edu.wpi.first.math.geometry.Pose3d;
 
-public record FastLimelightResults(double latency, List<PoseWithTagID> tags) {}
+public record FastLimelightResults(
+    double latency, Pose3d robotPose, double distanceToTag, boolean valid) {}