vision config + localization

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

@@ -98,7 +98,12 @@ 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,
@@ -50,6 +51,11 @@ public record ArmConfig(
       Consumer<InterpolatingDoubleTreeMap> speakerDistanceToAngle,
       double minAngle,
       double maxAngle) {}
+      public record VisionConfig(
+        int translationHistoryArraySize,
+        double xyStdDev,
+        double thetaStdDev,
+        Consumer<InterpolatingDoubleTreeMap> tyToNoteDistance) {}
 
   // TODO: Change this to false during events
   public static final boolean IS_DEVELOPMENT = true;

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,60 @@
+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.kinematics.SwerveModulePosition;
+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.VisionSubsystem;
+import frc.robot.vision.LimelightHelpers.PoseEstimate;
+
+public class LocalizationSubsystem extends StateMachine<LocalizationState>{
+  private final ImuSubsystem imu;
+  private final VisionSubsystem vision;
+  private final SwerveDrivePoseEstimator poseEstimator;
+  private PoseEstimate estimatepose;
+    private double lastAddedVisionTimestamp = 0;
+
+
+
+    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(){
+      var maybeResults = vision.getVisionResult();
+      if (maybeResults.isPresent()) {
+        var results = maybeResults.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;
+        }
+
+    }
+
+}
+}

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

@@ -55,4 +55,5 @@ public Optional<VisionResult> getVisionResult() {
   public void robotPeriodic() {
     super.robotPeriodic();
   }
+
 }