Visionconfig (#27)

* vision config + localization

* Adjust vision config

* Changes

* vision configs + localization

---------

Co-authored-by: Jordan <[email protected]>

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";
@@ -98,7 +99,10 @@ class CompConfig {
                 speakerDistanceToAngle.put(123.0, 321.0);
               },
               0.0,
-              30.0)); // NOT TUNED
+              30.0),
+          new VisionConfig(4, 0.4, 0.4));
+
+  // NOT TUNED
 
   private CompConfig() {}
 }

src/main/java/frc/robot/config/RobotConfig.java

@@ -12,7 +12,8 @@ public record RobotConfig(
     QueuerConfig queuer,
     ShooterConfig shooter,
     IntakeConfig intake,
-    ArmConfig arm) {
+    ArmConfig arm,
+    VisionConfig vision) {
   public record SwerveConfig(
       PhoenixPIDController snapController,
       boolean invertRotation,
@@ -51,6 +52,9 @@ public record ArmConfig(
       double minAngle,
       double maxAngle) {}
 
+  public record VisionConfig(
+      int translationHistoryArraySize, double xyStdDev, double thetaStdDev) {}
+
   // TODO: Change this to false during events
   public static final boolean IS_DEVELOPMENT = true;
   public static final String SERIAL_NUMBER = System.getenv("serialnum");

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

@@ -0,0 +1,5 @@
+package frc.robot.localization;
+
+public enum LocalizationState {
+  DEFAULT_STATE;
+}

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

@@ -0,0 +1,67 @@
+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.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.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);
+    this.imu = imu;
+    this.vision = vision;
+    poseEstimator = new SwerveDrivePoseEstimator(null, imu.getRobotHeading(), null, new Pose2d());
+  }
+
+  @Override
+  protected void collectInputs() {
+    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();
+
+      if (visionTimestamp == lastAddedVisionTimestamp) {
+        // Don't add the same vision pose over and over
+      } else {
+        poseEstimator.addVisionMeasurement(
+            visionPose,
+            visionTimestamp,
+            VecBuilder.fill(
+                RobotConfig.get().vision().xyStdDev(),
+                RobotConfig.get().vision().xyStdDev(),
+                RobotConfig.get().vision().thetaStdDev()));
+        lastAddedVisionTimestamp = visionTimestamp;
+      }
+    }
+
+    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
+  }
+}

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

@@ -43,10 +43,12 @@ protected void collectInputs() {
       processedVisionResult = Optional.empty();
     } else {
       var rawData = rawVisionResult.get();
-      // TODO: We don't do anything with this VisionResult??
-    processedVisionResult =  Optional.of(new VisionResult(VisionUtil.interpolatePose(rawData.pose()), rawData.timestamp()));
+      processedVisionResult =
+          Optional.of(
+              new VisionResult(VisionUtil.interpolatePose(rawData.pose()), rawData.timestamp()));
     }
   }
+
   public Optional<VisionResult> getVisionResult() {
     return processedVisionResult;
   }