From b1c8149b33441a05ea6458320ba89c9ad5be2e2b Mon Sep 17 00:00:00 2001
From: Jordan <117786700+jordanjcoderman@users.noreply.github.com>
Date: Tue, 3 Sep 2024 18:34:13 -0700
Subject: [PATCH] vision configs + localization

---
 .../java/frc/robot/config/CompConfig.java     |  1 +
 .../java/frc/robot/config/RobotConfig.java    |  4 +--
 .../localization/LocalizationSubsystem.java   | 31 +++++++++++++------
 3 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/config/CompConfig.java b/src/main/java/frc/robot/config/CompConfig.java
index d03073c..209f0d1 100644
--- a/src/main/java/frc/robot/config/CompConfig.java
+++ b/src/main/java/frc/robot/config/CompConfig.java
@@ -12,6 +12,7 @@
 import frc.robot.config.RobotConfig.QueuerConfig;
 import frc.robot.config.RobotConfig.ShooterConfig;
 import frc.robot.config.RobotConfig.SwerveConfig;
+import frc.robot.config.RobotConfig.VisionConfig;
 
 class CompConfig {
   private static final String CANIVORE_NAME = "581CANivore";
diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java
index 89e4f3d..1d158ec 100644
--- a/src/main/java/frc/robot/config/RobotConfig.java
+++ b/src/main/java/frc/robot/config/RobotConfig.java
@@ -53,9 +53,7 @@ public record ArmConfig(
       double maxAngle) {}
 
   public record VisionConfig(
-      int translationHistoryArraySize,
-      double xyStdDev,
-      double thetaStdDev) {}
+      int translationHistoryArraySize, double xyStdDev, double thetaStdDev) {}
 
   // TODO: Change this to false during events
   public static final boolean IS_DEVELOPMENT = true;
diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java
index 075859c..4448e29 100644
--- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java
+++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java
@@ -3,18 +3,24 @@
 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.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.config.RobotConfig;
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
-import frc.robot.vision.LimelightHelpers.PoseEstimate;
+import frc.robot.vision.VisionResult;
 import frc.robot.vision.VisionSubsystem;
+import java.util.Optional;
 
 public class LocalizationSubsystem extends StateMachine<LocalizationState> {
   private final ImuSubsystem imu;
   private final VisionSubsystem vision;
   private final SwerveDrivePoseEstimator poseEstimator;
+  private final TimeInterpolatableBuffer<Pose2d> poseHistory =
+      TimeInterpolatableBuffer.createBuffer(1.5);
   private double lastAddedVisionTimestamp = 0;
+  private Optional<VisionResult> latestResult = Optional.empty();
 
   public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision) {
     super(SubsystemPriority.LOCALIZATION, LocalizationState.DEFAULT_STATE);
@@ -25,9 +31,19 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision) {
 
   @Override
   protected void collectInputs() {
-    var maybeResults = vision.getVisionResult();
-    if (maybeResults.isPresent()) {
-      var results = maybeResults.get();
+    latestResult = vision.getVisionResult();
+  }
+
+  public Pose2d getPose() {
+    return poseEstimator.getEstimatedPosition();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    super.robotPeriodic();
+
+    if (latestResult.isPresent()) {
+      var results = latestResult.get();
       Pose2d visionPose = results.pose();
 
       double visionTimestamp = results.timestamp();
@@ -45,12 +61,7 @@ protected void collectInputs() {
         lastAddedVisionTimestamp = visionTimestamp;
       }
     }
-    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
-    PoseEstimate mt2Estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("");
-    vision.setRobotPose(getPose());
-  }
 
-  public Pose2d getPose() {
-    return poseEstimator;
+    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
   }
 }