Add PoseEstimation class

src/main/java/frc/robot/PoseEstimation.java

@@ -0,0 +1,66 @@
+package frc.robot;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.swerve.SwerveDrive;
+import frc.robot.vision.Vision;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+public class PoseEstimation {
+    private static PoseEstimation INSTANCE = null;
+    private final SwerveDrivePoseEstimator estimator;
+    private final Vision vision = Vision.getInstance();
+    private final SwerveDrive swerveDrive = SwerveDrive.getInstance();
+
+    public PoseEstimation() {
+        estimator =
+                new SwerveDrivePoseEstimator(
+                        swerveDrive.getKinematics(),
+                        swerveDrive.getYaw(),
+                        swerveDrive.getModulePositions(),
+                        swerveDrive.getBotPose());
+    }
+
+    public static PoseEstimation getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new PoseEstimation();
+        }
+        return INSTANCE;
+    }
+
+    public void addVisionMeasurement(
+            DoubleSupplier std1, DoubleSupplier std2, DoubleSupplier std3) {
+        var results = vision.getResults();
+        for (org.photonvision.EstimatedRobotPose result : results) {
+            estimator.addVisionMeasurement(
+                    result.estimatedPose.toPose2d(),
+                    result.timestampSeconds,
+                    VecBuilder.fill(std1.getAsDouble(), std2.getAsDouble(), std3.getAsDouble()));
+        }
+    }
+
+    public void updatePose() {
+        for (int i = 0; i < swerveDrive.getHighFreqModulePositions().size(); i++) {
+            estimator.updateWithTime(
+                    swerveDrive.getHighFreqTimeStamps()[i],
+                    swerveDrive.getYaw(),
+                    swerveDrive.getHighFreqModulePositions().get(i));
+        }
+    }
+
+    public void resetPose(Pose2d pose) {
+        estimator.resetPosition(
+                swerveDrive.getRawYaw(),
+                swerveDrive
+                        .getHighFreqModulePositions()
+                        .get(swerveDrive.getHighFreqModulePositions().size() - 1),
+                pose);
+    }
+
+    @AutoLogOutput(key = "EstimatedRobotPose")
+    public Pose2d getEstimatedPose() {
+        return estimator.getEstimatedPosition();
+    }
+}

src/main/java/frc/robot/swerve/SwerveDrive.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -14,7 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc.robot.PoseEstimation;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
@@ -35,6 +34,7 @@ public class SwerveDrive extends SubsystemBase {
 
     @AutoLogOutput
     private final SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
+
     private final List<SwerveModulePosition[]> highFreqModulePositions = new ArrayList<>();
 
     private final GyroIO gyro;
@@ -46,7 +46,7 @@ public class SwerveDrive extends SubsystemBase {
                     SwerveConstants.WHEEL_POSITIONS[3]);
     private final Derivative acceleration = new Derivative();
     private final LinearFilter accelFilter = LinearFilter.movingAverage(15);
-    private final SwerveDrivePoseEstimator poseEstimator;
+    private final PoseEstimation poseEstimator;
     private final SwerveDriveInputsAutoLogged loggerInputs = new SwerveDriveInputsAutoLogged();
     @AutoLogOutput private Pose2d botPose = new Pose2d();
 
@@ -59,8 +59,7 @@ private SwerveDrive(GyroIO gyroIO, ModuleIO... moduleIOs) {
 
         updateModulePositions();
 
-        poseEstimator =
-                new SwerveDrivePoseEstimator(kinematics, getRawYaw(), modulePositions, botPose);
+        poseEstimator = new PoseEstimation();
     }
 
     public static SwerveDrive getInstance() {
@@ -126,7 +125,7 @@ public SwerveModulePosition[] getModulePositions() {
         return modulePositions;
     }
 
-    public List<SwerveModulePosition[]> getHighFreqModulePositions(){
+    public List<SwerveModulePosition[]> getHighFreqModulePositions() {
         return highFreqModulePositions;
     }
 
@@ -142,6 +141,10 @@ public ChassisSpeeds getCurrentSpeeds() {
         return loggerInputs.currentSpeeds;
     }
 
+    public double[] getHighFreqTimeStamps() {
+        return modules[0].getHighFreqTimestamps();
+    }
+
     public void updateHighFreqPose() {
         int sampleCount = Integer.MAX_VALUE;
         for (int i = 0; i < 4; i++) {
@@ -158,13 +161,11 @@ public void updateHighFreqPose() {
                                 modules[moduleIndex].getHighFreqDriveDistances()[sampleIndex],
                                 Rotation2d.fromRadians(
                                         modules[moduleIndex].getHighFreqAngles()[sampleIndex]));
-                highFreqModulePositions.get(sampleIndex)[moduleIndex] = tempHighFreqModulePositions[moduleIndex];
+                highFreqModulePositions.get(sampleIndex)[moduleIndex] =
+                        tempHighFreqModulePositions[moduleIndex];
             }
-            poseEstimator.updateWithTime(
-                    modules[0].getHighFreqTimestamps()[sampleIndex],
-                    getRawYaw(),
-                    tempHighFreqModulePositions);
-            botPose = poseEstimator.getEstimatedPosition();
+            poseEstimator.updatePose();
+            botPose = poseEstimator.getEstimatedPose();
         }
     }
 
@@ -174,7 +175,7 @@ public Pose2d getBotPose() {
 
     public void resetPose(Pose2d pose) {
         botPose = pose;
-        poseEstimator.resetPosition(getRawYaw(), modulePositions, pose);
+        poseEstimator.resetPose(pose);
     }
 
     public void resetPose() {