From 68e687255841f9642418531375e2dfbe0e068c68 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Sat, 25 Jan 2025 15:23:41 -0500
Subject: [PATCH 01/21] Started adding the vision subsystem

---
 .../frc/robot/constants/VisionConstants.java  |  8 ++-
 .../subsystems/vision/VisionSubsystem.java    | 68 ++++++++++++++++++-
 2 files changed, 73 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index c6ad9140..fd6724f7 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -24,7 +24,6 @@ public final class VisionConstants {
   //   public static final double kThetaStdDevRejected = Units.degreesToRadians(360);
   //   public static final double kThetaStdThres = 0.2;
 
-
   // Velocity Filter
   public static final double kLinearCoeffOnVelFilter = 0.1;
   public static final double kOffsetOnVelFilter = 0.10;
@@ -50,7 +49,12 @@ public final class VisionConstants {
   public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0;
 
   // Constants for cameras
-  public static final int kNumCams = 4;
+  public static final int kNumCams = 5;
+  public static final int kNumPis = 3;
+  public static final int[] kUdpIndex = {0, 1, 2};
+
+  // Camera Ports
+  public static final int[] kCamPorts = {5802, 5802, 5803, 5803, 5804};
 
   // Names
   public static final String kCam1Name = "Shooter";
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 786ee396..8ac597e9 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -1,15 +1,28 @@
 package frc.robot.subsystems.vision;
 
+import WallEye.*;
+import WallEye.UdpSubscriber;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+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.ChassisSpeeds;
 import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.drive.Swerve;
+import java.io.IOException;
 import java.util.Set;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
-import frc.robot.subsystems.drive.DriveSubsystem;
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
+  WallEyeCam[] cams;
+
   Translation2d[] camPositions = {
     VisionConstants.kCam1Pose.getTranslation().toTranslation2d(),
     VisionConstants.kCam2Pose.getTranslation().toTranslation2d(),
@@ -39,9 +52,62 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam4Idx
   };
 
+  private Swerve swerve = new Swerve();
+  private DriveSubsystem driveSubsystem = new DriveSubsystem(swerve);
+  private Logger logger;
+  private UdpSubscriber[] udpSubscriber;
+  private Matrix adaptiveMatrix;
+  private AprilTagFieldLayout field;
+  private boolean updating = true;
+  private int minTags;
+
   public VisionSubsystem(DriveSubsystem driveSubsystem) {
+    this.driveSubsystem = driveSubsystem;
+    logger = LoggerFactory.getLogger("Vision");
+
+    cams = new WallEyeCam[VisionConstants.kNumCams];
+    udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis];
+    adaptiveMatrix = VisionConstants.kLocalMeasurementStdDevs.copy();
+    try {
+      field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile);
+    } catch (IOException e) {
+      logger.error("BAD NEWS BEARS NO APRIL TAG LAYOUT");
+    }
+
+    for (int i = 0; i < VisionConstants.kNumCams; i++) {
+      cams[i] = new WallEyeCam(piNames[i], camIndex[i], -1);
+    }
+
+    udpSubscriber[0] = new UdpSubscriber(0, cams[0], cams[1]);
+    udpSubscriber[1] = new UdpSubscriber(1, cams[2], cams[3]);
+    udpSubscriber[2] = new UdpSubscriber(2, cams[4]);
+  }
+
+  public void setVisionUpdating(boolean updating) {
+    this.updating = updating;
+  }
+
+  public boolean isVisionUpdating() {
+    return updating;
+  }
+
+  public void setMinTags(int minTags) {
+    this.minTags = minTags;
+  }
 
+  public boolean cameraConnected(int index) {
+    return cams[index].isCameraConnected();
   }
+
+  private boolean poseValidWithWheels(Translation2d pose, WallEyeResult result) {
+    if (poseValidWithWheels(pose, result)) {
+
+      ChassisSpeeds speed = driveSubsystem.getFieldRelSpeed();
+      Pose2d curPose = driveSubsystem.getPoseMeters();
+    }
+    return false;
+  }
+
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();

From 44189115347aa7c01b525cd3abea3692a1eeb9cb Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Mon, 27 Jan 2025 19:38:05 -0500
Subject: [PATCH 02/21] Started adding vision filters

---
 .../subsystems/vision/VisionSubsystem.java    | 39 ++++++++++++++++---
 1 file changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 8ac597e9..d89b224d 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -8,16 +8,22 @@
 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.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.Distance;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.Swerve;
+
+import static edu.wpi.first.units.Units.Meters;
+
 import java.io.IOException;
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
+import java.lang.Math;
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
@@ -99,15 +105,38 @@ public boolean cameraConnected(int index) {
     return cams[index].isCameraConnected();
   }
 
-  private boolean poseValidWithWheels(Translation2d pose, WallEyeResult result) {
-    if (poseValidWithWheels(pose, result)) {
+  //Filters
+  private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
 
-      ChassisSpeeds speed = driveSubsystem.getFieldRelSpeed();
+      ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed();
       Pose2d curPose = driveSubsystem.getPoseMeters();
-    }
-    return false;
+
+      Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d()));
+
+      double velMagnitude = Math.sqrt(Math.pow(vel.vxMetersPerSecond,2)
+      + Math.pow(vel.vyMetersPerSecond, 2));
+
+      double dispMagnitude = Math.sqrt(Math.pow(disp.getX(),2)+
+      Math.pow(disp.getY(),2));
+
+      return result.getNumTags() >= minTags &&
+      dispMagnitude <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter 
+      + VisionConstants.kOffsetOnVelFilter
+      + Math.pow(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter, 2)
+      );
+  }
+
+  private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
+
+    return (result.getNumTags() >= 2 
+    || result.getAmbiguity() < VisionConstants.kMaxAmbig) 
+    && pose.getMeasureX().in(Meters) < field.getFieldLength() 
+    && pose.getMeasureY().in(Meters) < field.getFieldWidth();
   }
 
+  
+
+
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();

From 4fc6b57527ce50290d4bffa91fed50681d7dae8f Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Mon, 27 Jan 2025 20:38:20 -0500
Subject: [PATCH 03/21] Added getter methods for tag info

---
 .../subsystems/vision/VisionSubsystem.java    | 82 ++++++++++++-------
 1 file changed, 52 insertions(+), 30 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index d89b224d..bbcd8fab 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.vision;
 
+import static edu.wpi.first.units.Units.*;
+
 import WallEye.*;
 import WallEye.UdpSubscriber;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -10,20 +12,15 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.units.measure.Distance;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.Swerve;
-
-import static edu.wpi.first.units.Units.Meters;
-
 import java.io.IOException;
 import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
-import java.lang.Math;
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
@@ -88,54 +85,79 @@ public VisionSubsystem(DriveSubsystem driveSubsystem) {
     udpSubscriber[1] = new UdpSubscriber(1, cams[2], cams[3]);
     udpSubscriber[2] = new UdpSubscriber(2, cams[4]);
   }
-
+  // Setter Methods
   public void setVisionUpdating(boolean updating) {
     this.updating = updating;
   }
 
-  public boolean isVisionUpdating() {
-    return updating;
-  }
-
   public void setMinTags(int minTags) {
     this.minTags = minTags;
   }
+  // Getter Methods
+  public boolean isVisionUpdating() {
+    return updating;
+  }
 
   public boolean cameraConnected(int index) {
     return cams[index].isCameraConnected();
   }
 
-  //Filters
-  private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
+  private double minTagDistance(WallEyePoseResult result) {
 
-      ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed();
-      Pose2d curPose = driveSubsystem.getPoseMeters();
+    Translation2d camLoc = result.getCameraPose().getTranslation().toTranslation2d();
+    int[] ids = result.getTagIDs();
+    double minDistance = 2767;
 
-      Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d()));
+    for (int id : ids) {
+      Translation2d tagLoc = field.getTagPose(id).get().getTranslation().toTranslation2d();
+      double dist = camLoc.getDistance(camLoc);
 
-      double velMagnitude = Math.sqrt(Math.pow(vel.vxMetersPerSecond,2)
-      + Math.pow(vel.vyMetersPerSecond, 2));
+      if (dist < minDistance) {
+        minDistance = dist;
+      }
+    }
+    return minDistance;
+  }
+
+  private double averageTagDistance(WallEyePoseResult result) {
 
-      double dispMagnitude = Math.sqrt(Math.pow(disp.getX(),2)+
-      Math.pow(disp.getY(),2));
+    Translation2d camLoc = result.getCameraPose().getTranslation().toTranslation2d();
+    int[] ids = result.getTagIDs();
+    double totalDistance = 0.0;
 
-      return result.getNumTags() >= minTags &&
-      dispMagnitude <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter 
-      + VisionConstants.kOffsetOnVelFilter
-      + Math.pow(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter, 2)
-      );
+    for (int id : ids) {
+      totalDistance +=
+          camLoc.getDistance(field.getTagPose(id).get().getTranslation().toTranslation2d());
+    }
+    return totalDistance / ids.length;
   }
 
-  private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
+  // Filters
+  private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
 
-    return (result.getNumTags() >= 2 
-    || result.getAmbiguity() < VisionConstants.kMaxAmbig) 
-    && pose.getMeasureX().in(Meters) < field.getFieldLength() 
-    && pose.getMeasureY().in(Meters) < field.getFieldWidth();
+    ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed();
+    Pose2d curPose = driveSubsystem.getPoseMeters();
+
+    Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d()));
+
+    double velMagnitude =
+        Math.sqrt(Math.pow(vel.vxMetersPerSecond, 2) + Math.pow(vel.vyMetersPerSecond, 2));
+
+    double dispMagnitude = Math.sqrt(Math.pow(disp.getX(), 2) + Math.pow(disp.getY(), 2));
+
+    return result.getNumTags() >= minTags
+        && dispMagnitude
+            <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter
+                + VisionConstants.kOffsetOnVelFilter
+                + Math.pow(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter, 2));
   }
 
-  
+  private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
 
+    return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig)
+        && pose.getMeasureX().in(Meters) < field.getFieldLength()
+        && pose.getMeasureY().in(Meters) < field.getFieldWidth();
+  }
 
   @Override
   public Set<Measure> getMeasures() {

From 52e57e310070a48f13cf93d34c7d2e3471b16861 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Tue, 28 Jan 2025 20:05:56 -0500
Subject: [PATCH 04/21] Added the function for standard deviation finding

---
 .../frc/robot/constants/VisionConstants.java  | 41 +++++---
 .../subsystems/vision/VisionSubsystem.java    | 99 ++++++++++++++++++-
 2 files changed, 124 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index fd6724f7..ca24b787 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -38,15 +38,27 @@ public final class VisionConstants {
   public static final double multiTagCoeff = 18.0 / 100.0;
   public static final double baseNumber = Math.E;
   public static final double powerNumber = 4.0;
+  public static final double baseTrust = 3.0;
+
   public static final double FOV45MultiTagCoeff = 16.0 / 100.0;
   public static final double FOV45powerNumber = 4.5;
-  public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0;
+  public static final double FOV45SingleTagCoeff = 21.0 / 100.0;
+  public static final double FOV45BaseTrust = 3.0;
+
   public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0;
   public static final double FOV58MJPGPowerNumber = 3.5;
   public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0;
+  public static final double FOV58MJPGBaseTrust = 3.0;
+
   public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0;
   public static final double FOV58YUYVPowerNumber = 4.0;
   public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0;
+  public static final double FOV58YUYVBaseTrust = 3.0;
+
+  public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0;
+  public static final double FOV75YUYVPowerNumber = 4.0;
+  public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0;
+  public static final double FOV75YUYVBaseTrust = 3.0;
 
   // Constants for cameras
   public static final int kNumCams = 5;
@@ -57,20 +69,22 @@ public final class VisionConstants {
   public static final int[] kCamPorts = {5802, 5802, 5803, 5803, 5804};
 
   // Names
-  public static final String kCam1Name = "Shooter";
-  public static final String kCam2Name = "Intake";
-  public static final String kCam3Name = "AngledShooterLeft";
-  public static final String kCam4Name = "AngledShooterRight";
+  public static final String kCam1Name = "Upper 1";
+  public static final String kCam2Name = "Upper 2";
+  public static final String kCam3Name = "Rear";
+  public static final String kCam4Name = "Servo Left";
+  public static final String kCam5Name = "Servo Right";
 
-  public static final String kPi1Name = "Shooter";
-  public static final String kPi2Name = "Intake";
-  public static final String kPi3Name = "AngledShooters";
+  public static final String kPi1Name = "Upper 1/Servo 1";
+  public static final String kPi2Name = "Cam 2/Servo 2";
+  public static final String kPi3Name = "Cam 3";
 
   // Indexs
   public static final int kCam1Idx = 0;
-  public static final int kCam2Idx = 0;
-  public static final int kCam3Idx = 0;
-  public static final int kCam4Idx = 1;
+  public static final int kCam2Idx = 1;
+  public static final int kCam3Idx = 2;
+  public static final int kCam4Idx = 0;
+  public static final int kCam5Idx = 1;
 
   public static final double kLoopTime = 0.02;
   public static final int kCircularBufferSize = 1000;
@@ -91,7 +105,10 @@ public final class VisionConstants {
       new Pose3d(
           new Translation3d(-0.22, -0.335, 0.50),
           new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
-
+  public static final Pose3d kCam5Pose =
+      new Pose3d(
+          new Translation3d(-0.22, -0.335, 0.50),
+          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
   // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
   // in the form [theta], with units in radians.
   public static Matrix<N1, N1> kLocalMeasurementStdDevs =
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index bbcd8fab..7d0770ca 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -17,6 +18,7 @@
 import frc.robot.subsystems.drive.Swerve;
 import java.io.IOException;
 import java.util.Set;
+import net.jafama.FastMath;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
@@ -30,18 +32,24 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam1Pose.getTranslation().toTranslation2d(),
     VisionConstants.kCam2Pose.getTranslation().toTranslation2d(),
     VisionConstants.kCam3Pose.getTranslation().toTranslation2d(),
-    VisionConstants.kCam4Pose.getTranslation().toTranslation2d()
+    VisionConstants.kCam4Pose.getTranslation().toTranslation2d(),
+    VisionConstants.kCam5Pose.getTranslation().toTranslation2d()
   };
 
   Rotation2d[] camRotations = {
     VisionConstants.kCam1Pose.getRotation().toRotation2d(),
     VisionConstants.kCam2Pose.getRotation().toRotation2d(),
     VisionConstants.kCam3Pose.getRotation().toRotation2d(),
-    VisionConstants.kCam4Pose.getRotation().toRotation2d()
+    VisionConstants.kCam4Pose.getRotation().toRotation2d(),
+    VisionConstants.kCam5Pose.getRotation().toRotation2d()
   };
 
   String[] camNames = {
-    VisionConstants.kCam1Name, VisionConstants.kCam2Name, VisionConstants.kCam3Name
+    VisionConstants.kCam1Name,
+    VisionConstants.kCam2Name,
+    VisionConstants.kCam3Name,
+    VisionConstants.kCam4Name,
+    VisionConstants.kCam5Name
   };
 
   String[] piNames = {
@@ -52,7 +60,8 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam1Idx,
     VisionConstants.kCam2Idx,
     VisionConstants.kCam3Idx,
-    VisionConstants.kCam4Idx
+    VisionConstants.kCam4Idx,
+    VisionConstants.kCam5Idx
   };
 
   private Swerve swerve = new Swerve();
@@ -159,6 +168,88 @@ private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
         && pose.getMeasureY().in(Meters) < field.getFieldWidth();
   }
 
+  private double getStdDevFactor(double distance, int numTags, String camName) {
+    switch (camName) {
+      case "Upper Right":
+      case "Upper Left":
+        if (numTags == 1)
+          return 1.0
+              / VisionConstants.FOV58YUYVBaseTrust
+              * FastMath.pow(
+                  VisionConstants.baseNumber,
+                  FastMath.pow(
+                      VisionConstants.FOV58YUYVSingleTagCoeff * distance,
+                      VisionConstants.FOV58YUYVPowerNumber));
+
+        return 1
+            / VisionConstants.FOV58YUYVBaseTrust
+            * FastMath.pow(
+                VisionConstants.baseNumber,
+                FastMath.pow(
+                    VisionConstants.FOV58YUYVMultiTagCoeff * distance,
+                    VisionConstants.FOV58YUYVPowerNumber));
+
+      case "Rear":
+        if (numTags == 1)
+          return 1
+              / VisionConstants.FOV58YUYVBaseTrust
+              * FastMath.pow(
+                  VisionConstants.baseNumber,
+                  FastMath.pow(
+                      VisionConstants.FOV58MJPGSingleTagCoeff * distance,
+                      VisionConstants.FOV58YUYVPowerNumber));
+
+        return 1
+            / VisionConstants.FOV58YUYVBaseTrust
+            * FastMath.pow(
+                VisionConstants.baseNumber,
+                FastMath.pow(
+                    VisionConstants.FOV58MJPGSingleTagCoeff * distance,
+                    VisionConstants.FOV58YUYVPowerNumber));
+
+      case "Servo Left":
+      case "Servo Right":
+        if (numTags == 1)
+          return 1
+              / VisionConstants.FOV75YUYVBaseTrust
+              * FastMath.pow(
+                  VisionConstants.baseNumber,
+                  FastMath.pow(
+                      VisionConstants.FOV75YUYVSingleTagCoeff * distance,
+                      VisionConstants.FOV75YUYVPowerNumber));
+        return 1
+            / VisionConstants.FOV75YUYVBaseTrust
+            * FastMath.pow(
+                VisionConstants.baseNumber,
+                FastMath.pow(
+                    VisionConstants.FOV75YUYVMultiTagCoeff * distance,
+                    VisionConstants.FOV75YUYVPowerNumber));
+
+      default:
+        if (numTags == 1)
+          return 1
+              / VisionConstants.baseTrust
+              * FastMath.pow(
+                  VisionConstants.baseNumber,
+                  FastMath.pow(
+                      VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber));
+        return 1
+            / VisionConstants.baseTrust
+            * FastMath.pow(
+                VisionConstants.baseNumber,
+                FastMath.pow(
+                    VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber));
+    }
+  }
+
+  private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
+    if (Math.abs(new Rotation2d(rotation).minus(pose1.getRotation().toRotation2d()).getRadians())
+        <= Math.abs(
+            new Rotation2d(rotation).minus(pose2.getRotation().toRotation2d()).getRadians()))
+      return pose1;
+    else return pose2;
+  }
+
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();

From 24297e566c14e25984733b7931fbe80f9196a8ae Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 30 Jan 2025 20:02:03 -0500
Subject: [PATCH 05/21] Finished adding filters and started working on periodic

---
 .../subsystems/vision/VisionSubsystem.java    | 58 +++++++++++++++++++
 1 file changed, 58 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 7d0770ca..ffba779b 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -7,16 +7,20 @@
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.util.CircularBuffer;
+import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.drive.Swerve;
 import java.io.IOException;
+import java.util.ArrayList;
 import java.util.Set;
 import net.jafama.FastMath;
 import org.slf4j.Logger;
@@ -44,6 +48,14 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Pose.getRotation().toRotation2d()
   };
 
+  private double[] camHeights = {
+    VisionConstants.kCam1Pose.getMeasureZ().in(Meters),
+    VisionConstants.kCam2Pose.getMeasureZ().in(Meters),
+    VisionConstants.kCam3Pose.getMeasureZ().in(Meters),
+    VisionConstants.kCam4Pose.getMeasureZ().in(Meters),
+    VisionConstants.kCam5Pose.getMeasureZ().in(Meters)
+  };
+
   String[] camNames = {
     VisionConstants.kCam1Name,
     VisionConstants.kCam2Name,
@@ -72,6 +84,11 @@ public class VisionSubsystem extends MeasurableSubsystem {
   private AprilTagFieldLayout field;
   private boolean updating = true;
   private int minTags;
+  private CircularBuffer<Double> gyroBuffer =
+      new CircularBuffer<Double>(VisionConstants.kCircularBufferSize);
+  private double timeSinceLastUpdate;
+  private int updatesToWheels;
+  private ArrayList<Pair<WallEyeResult, Integer>> validResults = new ArrayList<>();
 
   public VisionSubsystem(DriveSubsystem driveSubsystem) {
     this.driveSubsystem = driveSubsystem;
@@ -111,6 +128,10 @@ public boolean cameraConnected(int index) {
     return cams[index].isCameraConnected();
   }
 
+  private double getSeconds() {
+    return RobotController.getFPGATime();
+  }
+
   private double minTagDistance(WallEyePoseResult result) {
 
     Translation2d camLoc = result.getCameraPose().getTranslation().toTranslation2d();
@@ -250,6 +271,43 @@ private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
     else return pose2;
   }
 
+  private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) {
+    double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
+    double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
+
+    if (dist1 < dist2 && dist1 < 0.5 && dist1 > 0) {
+      return pose1;
+    }
+    if (dist2 < dist1 && dist2 < 0.5 && dist2 > 0) {
+      return pose2;
+    }
+
+    if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1;
+
+    double rotation =
+        gyroBuffer.get(FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime)));
+    return getCloserPose(pose1, pose2, rotation);
+  }
+
+  @Override
+  public void periodic() {
+    gyroBuffer.addFirst(
+        FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()));
+
+    if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) {
+      updatesToWheels = 0;
+    }
+
+    validResults.clear();
+
+    for (int i = 0; i < VisionConstants.kNumCams; i++) {
+      if (cams[i].hasNewUpdate()) {
+        timeSinceLastUpdate = getSeconds();
+        validResults.add(new Pair<WallEyeResult, Integer>(cams[i].getResults(), i));
+      }
+    }
+  }
+
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();

From 1925044f6cf7c564331e364654213c46c36b7e32 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Sat, 1 Feb 2025 14:46:23 -0500
Subject: [PATCH 06/21] Continued to work on the periodic

---
 .../subsystems/vision/VisionSubsystem.java    | 87 +++++++++++++++----
 1 file changed, 70 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index ffba779b..aa1164ae 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -3,7 +3,6 @@
 import static edu.wpi.first.units.Units.*;
 
 import WallEye.*;
-import WallEye.UdpSubscriber;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
@@ -11,9 +10,11 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.util.CircularBuffer;
 import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.constants.VisionConstants;
@@ -27,25 +28,26 @@
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
+import edu.wpi.first.math.Matrix;
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
   WallEyeCam[] cams;
 
-  Translation2d[] camPositions = {
-    VisionConstants.kCam1Pose.getTranslation().toTranslation2d(),
-    VisionConstants.kCam2Pose.getTranslation().toTranslation2d(),
-    VisionConstants.kCam3Pose.getTranslation().toTranslation2d(),
-    VisionConstants.kCam4Pose.getTranslation().toTranslation2d(),
-    VisionConstants.kCam5Pose.getTranslation().toTranslation2d()
+  Translation3d[] camPositions = {
+    VisionConstants.kCam1Pose.getTranslation(),
+    VisionConstants.kCam2Pose.getTranslation(),
+    VisionConstants.kCam3Pose.getTranslation(),
+    VisionConstants.kCam4Pose.getTranslation(),
+    VisionConstants.kCam5Pose.getTranslation()
   };
 
-  Rotation2d[] camRotations = {
-    VisionConstants.kCam1Pose.getRotation().toRotation2d(),
-    VisionConstants.kCam2Pose.getRotation().toRotation2d(),
-    VisionConstants.kCam3Pose.getRotation().toRotation2d(),
-    VisionConstants.kCam4Pose.getRotation().toRotation2d(),
-    VisionConstants.kCam5Pose.getRotation().toRotation2d()
+  Rotation3d[] camRotations = {
+    VisionConstants.kCam1Pose.getRotation(),
+    VisionConstants.kCam2Pose.getRotation(),
+    VisionConstants.kCam3Pose.getRotation(),
+    VisionConstants.kCam4Pose.getRotation(),
+    VisionConstants.kCam5Pose.getRotation()
   };
 
   private double[] camHeights = {
@@ -80,15 +82,17 @@ public class VisionSubsystem extends MeasurableSubsystem {
   private DriveSubsystem driveSubsystem = new DriveSubsystem(swerve);
   private Logger logger;
   private UdpSubscriber[] udpSubscriber;
-  private Matrix adaptiveMatrix;
   private AprilTagFieldLayout field;
   private boolean updating = true;
   private int minTags;
   private CircularBuffer<Double> gyroBuffer =
       new CircularBuffer<Double>(VisionConstants.kCircularBufferSize);
   private double timeSinceLastUpdate;
-  private int updatesToWheels;
+  private int updatesToWheels;  private Matrix adaptiveMatrix;
   private ArrayList<Pair<WallEyeResult, Integer>> validResults = new ArrayList<>();
+  private WallEyeResult[] lastResult;
+  private Matrix<N3,N1> adativeMatrix;
+  private Matrix<N3,N1> stdMatrix;
 
   public VisionSubsystem(DriveSubsystem driveSubsystem) {
     this.driveSubsystem = driveSubsystem;
@@ -96,7 +100,8 @@ public VisionSubsystem(DriveSubsystem driveSubsystem) {
 
     cams = new WallEyeCam[VisionConstants.kNumCams];
     udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis];
-    adaptiveMatrix = VisionConstants.kLocalMeasurementStdDevs.copy();
+    adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
+    stdMatrix = adaptiveMatrix.copy();
     try {
       field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile);
     } catch (IOException e) {
@@ -306,8 +311,56 @@ public void periodic() {
         validResults.add(new Pair<WallEyeResult, Integer>(cams[i].getResults(), i));
       }
     }
-  }
 
+    if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) {
+      for (int i = 0; i < 2; i++){
+
+        double scaledWeight = VisionConstants.kVisionMeasurementStdDevs.get(i, 0)
+        + VisionConstants.kStdDevDecayCoeff
+        * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev);
+
+        adaptiveMatrix.set(i, 0, 
+        scaledWeight >= VisionConstants.kMinStdDev 
+          ? scaledWeight : VisionConstants.kMinStdDev);
+      }
+    }
+
+    for(Pair<WallEyeResult, Integer>res : validResults){
+      if(res.getFirst() instanceof WallEyeResult){
+        //adaptiveMatrix.set(0, 0, .1);
+        //adaptiveMatrix.set(1, 0, .1);
+
+        WallEyePoseResult result = (WallEyePoseResult) res.getFirst();
+        int idx = res.getSecond();
+
+        for (int i = 0; i < 2; i++){
+        
+        stdMatrix.set(i, 0,
+        .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx]));
+        }
+
+        Pose3d cameraPose;
+        Translation3d centerPose;
+        Rotation3d cameraRotation;
+
+        if(result.getNumTags() > 1){
+          cameraPose = result.getCameraPose();
+
+          centerPose = cameraPose.getTranslation()
+          .minus(camPositions[idx]
+            .rotateBy(cameraPose.getRotation()))
+            .rotateBy(camRotations[idx]);
+        }
+        else{
+          //Work on this on Monday
+        }
+      }
+    }
+
+
+
+  }
+ 
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();

From 87d9662c86b4249020ff3b4836a622da14417d76 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Mon, 3 Feb 2025 20:04:49 -0500
Subject: [PATCH 07/21] Finished main portion of vision. No logging yet

---
 .../subsystems/drive/DriveSubsystem.java      |  11 ++
 .../subsystems/vision/VisionSubsystem.java    | 111 ++++++++++++------
 2 files changed, 87 insertions(+), 35 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 79282da4..5dd283d5 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -2,6 +2,7 @@
 
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -9,6 +10,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc.robot.constants.DriveConstants;
 import java.util.Set;
@@ -158,6 +161,14 @@ public Trajectory<SwerveSample> getAutoTrajectory() {
     }
   }
 
+  public void addVisionMeasurement(Pose2d pose, double timestamp) {
+    io.addVisionMeasurement(pose, timestamp);
+  }
+
+  public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix<N3, N1> stdDevvs) {
+    io.addVisionMeasurement(pose, timestamp, stdDevvs);
+  }
+
   public void resetHolonomicController(double yaw) {
     xController.reset();
     yController.reset();
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index aa1164ae..c381b847 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -26,9 +26,9 @@
 import net.jafama.FastMath;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
-import edu.wpi.first.math.Matrix;
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
@@ -83,16 +83,17 @@ public class VisionSubsystem extends MeasurableSubsystem {
   private Logger logger;
   private UdpSubscriber[] udpSubscriber;
   private AprilTagFieldLayout field;
-  private boolean updating = true;
+  private boolean visionUpdating = true;
   private int minTags;
   private CircularBuffer<Double> gyroBuffer =
       new CircularBuffer<Double>(VisionConstants.kCircularBufferSize);
   private double timeSinceLastUpdate;
-  private int updatesToWheels;  private Matrix adaptiveMatrix;
+  private int updatesToWheels;
+  private Matrix adaptiveMatrix;
   private ArrayList<Pair<WallEyeResult, Integer>> validResults = new ArrayList<>();
-  private WallEyeResult[] lastResult;
-  private Matrix<N3,N1> adativeMatrix;
-  private Matrix<N3,N1> stdMatrix;
+  private WallEyeTagResult[] lastResult;
+  private Matrix<N3, N1> adativeMatrix;
+  private Matrix<N3, N1> stdMatrix;
 
   public VisionSubsystem(DriveSubsystem driveSubsystem) {
     this.driveSubsystem = driveSubsystem;
@@ -118,7 +119,7 @@ public VisionSubsystem(DriveSubsystem driveSubsystem) {
   }
   // Setter Methods
   public void setVisionUpdating(boolean updating) {
-    this.updating = updating;
+    this.visionUpdating = updating;
   }
 
   public void setMinTags(int minTags) {
@@ -126,7 +127,7 @@ public void setMinTags(int minTags) {
   }
   // Getter Methods
   public boolean isVisionUpdating() {
-    return updating;
+    return visionUpdating;
   }
 
   public boolean cameraConnected(int index) {
@@ -313,56 +314,96 @@ public void periodic() {
     }
 
     if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) {
-      for (int i = 0; i < 2; i++){
+      for (int i = 0; i < 2; i++) {
 
-        double scaledWeight = VisionConstants.kVisionMeasurementStdDevs.get(i, 0)
-        + VisionConstants.kStdDevDecayCoeff
-        * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev);
+        double scaledWeight =
+            VisionConstants.kVisionMeasurementStdDevs.get(i, 0)
+                + VisionConstants.kStdDevDecayCoeff
+                    * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev);
 
-        adaptiveMatrix.set(i, 0, 
-        scaledWeight >= VisionConstants.kMinStdDev 
-          ? scaledWeight : VisionConstants.kMinStdDev);
+        adaptiveMatrix.set(
+            i,
+            0,
+            scaledWeight >= VisionConstants.kMinStdDev ? scaledWeight : VisionConstants.kMinStdDev);
       }
     }
 
-    for(Pair<WallEyeResult, Integer>res : validResults){
-      if(res.getFirst() instanceof WallEyeResult){
-        //adaptiveMatrix.set(0, 0, .1);
-        //adaptiveMatrix.set(1, 0, .1);
+    for (Pair<WallEyeResult, Integer> res : validResults) {
+      if (res.getFirst() instanceof WallEyeResult) {
+        // I don't understand why we would do this. So i didn't
+        adaptiveMatrix.set(0, 0, .1);
+        adaptiveMatrix.set(1, 0, .1);
 
         WallEyePoseResult result = (WallEyePoseResult) res.getFirst();
         int idx = res.getSecond();
 
-        for (int i = 0; i < 2; i++){
-        
-        stdMatrix.set(i, 0,
-        .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx]));
+        for (int i = 0; i < 2; i++) {
+
+          stdMatrix.set(
+              i,
+              0,
+              .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx]));
         }
 
         Pose3d cameraPose;
-        Translation3d centerPose;
-        Rotation3d cameraRotation;
+        Translation3d centerPose = new Translation3d();
+        Rotation3d cameraRotation = new Rotation3d();
 
-        if(result.getNumTags() > 1){
+        if (result.getNumTags() > 1) {
           cameraPose = result.getCameraPose();
 
-          centerPose = cameraPose.getTranslation()
-          .minus(camPositions[idx]
-            .rotateBy(cameraPose.getRotation()))
-            .rotateBy(camRotations[idx]);
+          centerPose =
+              cameraPose
+                  .getTranslation()
+                  .minus(camPositions[idx].rotateBy(cameraPose.getRotation()))
+                  .rotateBy(camRotations[idx]);
+        } else {
+          Pose3d cam1Pose = result.getFirstPose();
+          Pose3d cam2Pose = result.getSecondPose();
+
+          cam1Pose =
+              new Pose3d(
+                  cam1Pose.getTranslation(), cam1Pose.getRotation().rotateBy(camRotations[idx]));
+          cam2Pose =
+              new Pose3d(
+                  cam2Pose.getTranslation(), cam2Pose.getRotation().rotateBy(camRotations[idx]));
+
+          cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx);
+          centerPose =
+              cameraPose
+                  .getTranslation()
+                  .minus(camPositions[idx].rotateBy(cameraPose.getRotation()))
+                  .rotateBy(camRotations[idx]);
+          cameraRotation = cameraPose.getRotation();
         }
-        else{
-          //Work on this on Monday
+        if (camsWithinField(centerPose, result)) {
+          updatesToWheels++;
+
+          if (visionUpdating) {
+            driveSubsystem.addVisionMeasurement(
+                new Pose2d(centerPose.toTranslation2d(), cameraRotation.toRotation2d()),
+                result.getTimeStamp(),
+                stdMatrix);
+          }
         }
+      } else if (res.getFirst() instanceof WallEyeTagResult) {
+        WallEyeTagResult tags = (WallEyeTagResult) res.getFirst();
+        int idx = res.getSecond();
       }
     }
+  }
 
-
-
+  public WallEyeTagResult getLastResult(int index) {
+    return lastResult[index];
   }
- 
+
   @Override
   public Set<Measure> getMeasures() {
     return Set.of();
   }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+  }
 }

From a08da7934fa3cf9417d2a528fb9e27b2bd3a04f6 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Tue, 4 Feb 2025 19:07:09 -0500
Subject: [PATCH 08/21] Adding logging and documentation

---
 .../frc/robot/constants/VisionConstants.java  |  1 +
 .../subsystems/vision/VisionSubsystem.java    | 77 ++++++++++++++-----
 2 files changed, 57 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index ca24b787..f08b0138 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -19,6 +19,7 @@ public final class VisionConstants {
   public static final double kMaxAmbig = 1.0;
   public static final int kMaxTimesOffWheels = 5;
   public static final double kBumperPixelLine = 87; // 100
+  public static final double kRobotHeight = 0.5;
 
   //   public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02);
   //   public static final double kThetaStdDevRejected = Units.degreesToRadians(360);
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index c381b847..9fd8d8a4 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -24,7 +24,7 @@
 import java.util.ArrayList;
 import java.util.Set;
 import net.jafama.FastMath;
-import org.slf4j.Logger;
+import org.littletonrobotics.junction.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
@@ -32,8 +32,10 @@
 
 public class VisionSubsystem extends MeasurableSubsystem {
 
+  // Array of cameras
   WallEyeCam[] cams;
 
+  // Array of camera positions
   Translation3d[] camPositions = {
     VisionConstants.kCam1Pose.getTranslation(),
     VisionConstants.kCam2Pose.getTranslation(),
@@ -42,6 +44,7 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Pose.getTranslation()
   };
 
+  // Array of camera rotations
   Rotation3d[] camRotations = {
     VisionConstants.kCam1Pose.getRotation(),
     VisionConstants.kCam2Pose.getRotation(),
@@ -50,6 +53,7 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Pose.getRotation()
   };
 
+  // Array of camera heights
   private double[] camHeights = {
     VisionConstants.kCam1Pose.getMeasureZ().in(Meters),
     VisionConstants.kCam2Pose.getMeasureZ().in(Meters),
@@ -58,6 +62,7 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Pose.getMeasureZ().in(Meters)
   };
 
+  // Array of camera names
   String[] camNames = {
     VisionConstants.kCam1Name,
     VisionConstants.kCam2Name,
@@ -66,10 +71,12 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Name
   };
 
+  // Array of orange pi names
   String[] piNames = {
     VisionConstants.kPi1Name, VisionConstants.kPi2Name, VisionConstants.kPi3Name,
   };
 
+  // Array of camera indexs
   int[] camIndex = {
     VisionConstants.kCam1Idx,
     VisionConstants.kCam2Idx,
@@ -80,7 +87,9 @@ public class VisionSubsystem extends MeasurableSubsystem {
 
   private Swerve swerve = new Swerve();
   private DriveSubsystem driveSubsystem = new DriveSubsystem(swerve);
-  private Logger logger;
+  /*Because we use two seperate loggers we can import one and then define the
+  other here.*/
+  private org.slf4j.Logger textLogger;
   private UdpSubscriber[] udpSubscriber;
   private AprilTagFieldLayout field;
   private boolean visionUpdating = true;
@@ -94,25 +103,29 @@ public class VisionSubsystem extends MeasurableSubsystem {
   private WallEyeTagResult[] lastResult;
   private Matrix<N3, N1> adativeMatrix;
   private Matrix<N3, N1> stdMatrix;
+  private Logger logger;
 
   public VisionSubsystem(DriveSubsystem driveSubsystem) {
     this.driveSubsystem = driveSubsystem;
-    logger = LoggerFactory.getLogger("Vision");
+    textLogger = LoggerFactory.getLogger("Vision");
 
     cams = new WallEyeCam[VisionConstants.kNumCams];
     udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis];
+    // We copy the matrix from our constants
     adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy();
+    // We then copy the adaptive matrix because they will differ later
     stdMatrix = adaptiveMatrix.copy();
+    // I'm not sure why we put this in a try catch maybe could be removed
     try {
       field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile);
     } catch (IOException e) {
-      logger.error("BAD NEWS BEARS NO APRIL TAG LAYOUT");
+      textLogger.error("BAD NEWS BEARS NO APRIL TAG LAYOUT");
     }
-
+    // Fill our camera array
     for (int i = 0; i < VisionConstants.kNumCams; i++) {
       cams[i] = new WallEyeCam(piNames[i], camIndex[i], -1);
     }
-
+    // Initialize our udpSubscribers
     udpSubscriber[0] = new UdpSubscriber(0, cams[0], cams[1]);
     udpSubscriber[1] = new UdpSubscriber(1, cams[2], cams[3]);
     udpSubscriber[2] = new UdpSubscriber(2, cams[4]);
@@ -142,11 +155,13 @@ private double minTagDistance(WallEyePoseResult result) {
 
     Translation2d camLoc = result.getCameraPose().getTranslation().toTranslation2d();
     int[] ids = result.getTagIDs();
+    // This number's value doesn't really matter it just needs to be large
     double minDistance = 2767;
 
+    // Go through the camera locations and finds the one closest to the tag
     for (int id : ids) {
-      Translation2d tagLoc = field.getTagPose(id).get().getTranslation().toTranslation2d();
-      double dist = camLoc.getDistance(camLoc);
+      double dist =
+          camLoc.getDistance(field.getTagPose(id).get().getTranslation().toTranslation2d());
 
       if (dist < minDistance) {
         minDistance = dist;
@@ -161,6 +176,7 @@ private double averageTagDistance(WallEyePoseResult result) {
     int[] ids = result.getTagIDs();
     double totalDistance = 0.0;
 
+    // Goes through the camera locations and gets the average distance
     for (int id : ids) {
       totalDistance +=
           camLoc.getDistance(field.getTagPose(id).get().getTranslation().toTranslation2d());
@@ -181,6 +197,9 @@ private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
 
     double dispMagnitude = Math.sqrt(Math.pow(disp.getX(), 2) + Math.pow(disp.getY(), 2));
 
+    /*This gets our displacement and compares it to who much we could
+    have moved.It does this by getting the velocity and plotting it on a
+    graph. The graph will be in the readme.*/
     return result.getNumTags() >= minTags
         && dispMagnitude
             <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter
@@ -189,18 +208,19 @@ private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
   }
 
   private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
-
     return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig)
         && pose.getMeasureX().in(Meters) < field.getFieldLength()
         && pose.getMeasureY().in(Meters) < field.getFieldWidth();
   }
 
+  /*Large switch case to see get the standard deviation factor based on camera, how many
+  tags we see and distance. An example of this graph will be in the readme.*/
   private double getStdDevFactor(double distance, int numTags, String camName) {
     switch (camName) {
       case "Upper Right":
       case "Upper Left":
         if (numTags == 1)
-          return 1.0
+          return 1
               / VisionConstants.FOV58YUYVBaseTrust
               * FastMath.pow(
                   VisionConstants.baseNumber,
@@ -270,6 +290,8 @@ private double getStdDevFactor(double distance, int numTags, String camName) {
   }
 
   private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
+    /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro
+    from the rotation of the pose and compare the two*/
     if (Math.abs(new Rotation2d(rotation).minus(pose1.getRotation().toRotation2d()).getRadians())
         <= Math.abs(
             new Rotation2d(rotation).minus(pose2.getRotation().toRotation2d()).getRadians()))
@@ -280,16 +302,17 @@ private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
   private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) {
     double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
     double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
-
-    if (dist1 < dist2 && dist1 < 0.5 && dist1 > 0) {
+    // This filters out results by seeing if they are the height of the robot.
+    if (dist1 < dist2 && dist1 < VisionConstants.kRobotHeight && dist1 > 0) {
       return pose1;
     }
-    if (dist2 < dist1 && dist2 < 0.5 && dist2 > 0) {
+    if (dist2 < dist1 && dist2 < VisionConstants.kRobotHeight && dist2 > 0) {
       return pose2;
     }
-
+    // If we don't have enough data in the gyro buffer we default to returning a pose
     if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1;
 
+    // See what pose is closer the the gyro at the time of the photo's capture.
     double rotation =
         gyroBuffer.get(FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime)));
     return getCloserPose(pose1, pose2, rotation);
@@ -297,8 +320,10 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn
 
   @Override
   public void periodic() {
-    gyroBuffer.addFirst(
-        FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()));
+
+    double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians());
+    gyroBuffer.addFirst(gyroData);
+    logger.recordOutput("Vision/Gyro Buffer", gyroData);
 
     if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) {
       updatesToWheels = 0;
@@ -314,8 +339,8 @@ public void periodic() {
     }
 
     if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) {
+      // Decrease the thresholds required for a good pose over time. A graph is in the readme
       for (int i = 0; i < 2; i++) {
-
         double scaledWeight =
             VisionConstants.kVisionMeasurementStdDevs.get(i, 0)
                 + VisionConstants.kStdDevDecayCoeff
@@ -330,15 +355,15 @@ public void periodic() {
 
     for (Pair<WallEyeResult, Integer> res : validResults) {
       if (res.getFirst() instanceof WallEyeResult) {
-        // I don't understand why we would do this. So i didn't
+        /*Reset the adaptive matrix to a stricter value once we get a result.
+        We set it to a stricter value then initially.*/
         adaptiveMatrix.set(0, 0, .1);
         adaptiveMatrix.set(1, 0, .1);
 
         WallEyePoseResult result = (WallEyePoseResult) res.getFirst();
+        logger.recordOutput("Vision", result.getTimeStamp());
         int idx = res.getSecond();
-
         for (int i = 0; i < 2; i++) {
-
           stdMatrix.set(
               i,
               0,
@@ -350,6 +375,7 @@ public void periodic() {
         Rotation3d cameraRotation = new Rotation3d();
 
         if (result.getNumTags() > 1) {
+          // If there our more then one tag in an image we can get pose possible pose
           cameraPose = result.getCameraPose();
 
           centerPose =
@@ -358,6 +384,7 @@ public void periodic() {
                   .minus(camPositions[idx].rotateBy(cameraPose.getRotation()))
                   .rotateBy(camRotations[idx]);
         } else {
+          // If there is one we get two possible poses and have to filter them.
           Pose3d cam1Pose = result.getFirstPose();
           Pose3d cam2Pose = result.getSecondPose();
 
@@ -368,6 +395,9 @@ public void periodic() {
               new Pose3d(
                   cam2Pose.getTranslation(), cam2Pose.getRotation().rotateBy(camRotations[idx]));
 
+          logger.recordOutput("Vision/Camera 1", cam1Pose);
+          logger.recordOutput("Vision/Camera 2", cam2Pose);
+
           cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx);
           centerPose =
               cameraPose
@@ -377,16 +407,21 @@ public void periodic() {
           cameraRotation = cameraPose.getRotation();
         }
         if (camsWithinField(centerPose, result)) {
+          // Is the pose in the field? If so, enjoy a updated position drive subsystem
           updatesToWheels++;
-
+          logger.recordOutput("Vision/Accepted Cam", centerPose);
+          // However we do have to be accepting the poses to use them
           if (visionUpdating) {
             driveSubsystem.addVisionMeasurement(
                 new Pose2d(centerPose.toTranslation2d(), cameraRotation.toRotation2d()),
                 result.getTimeStamp(),
                 stdMatrix);
           }
+        } else {
+          logger.recordOutput("Vision/Rejected Cam", centerPose);
         }
       } else if (res.getFirst() instanceof WallEyeTagResult) {
+        // I don't know what it does. I don't it does anything. But be careful about deletion.
         WallEyeTagResult tags = (WallEyeTagResult) res.getFirst();
         int idx = res.getSecond();
       }

From 1d5571b3c43203c5e9ee36fd1166a6025b95a8a4 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <111768816+Huck-Richardson@users.noreply.github.com>
Date: Tue, 4 Feb 2025 19:13:27 -0500
Subject: [PATCH 09/21] Fixed a few comments

---
 .../java/frc/robot/subsystems/vision/VisionSubsystem.java   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 9fd8d8a4..3c9691b8 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -199,7 +199,7 @@ private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
 
     /*This gets our displacement and compares it to who much we could
     have moved.It does this by getting the velocity and plotting it on a
-    graph. The graph will be in the readme.*/
+    graph. The graph will be in the docs.*/
     return result.getNumTags() >= minTags
         && dispMagnitude
             <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter
@@ -214,7 +214,7 @@ private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) {
   }
 
   /*Large switch case to see get the standard deviation factor based on camera, how many
-  tags we see and distance. An example of this graph will be in the readme.*/
+  tags we see and distance. An example of this graph will be in the docs.*/
   private double getStdDevFactor(double distance, int numTags, String camName) {
     switch (camName) {
       case "Upper Right":
@@ -339,7 +339,7 @@ public void periodic() {
     }
 
     if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) {
-      // Decrease the thresholds required for a good pose over time. A graph is in the readme
+      // Decrease the thresholds required for a good pose over time. A graph is in the docs
       for (int i = 0; i < 2; i++) {
         double scaledWeight =
             VisionConstants.kVisionMeasurementStdDevs.get(i, 0)

From 30b23a88ed436cc1ec5a5d551e53e67e73558b0e Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 6 Feb 2025 18:26:56 -0500
Subject: [PATCH 10/21] Fixed some minor issues

---
 .../robot/subsystems/vision/VisionSubsystem.java   | 14 ++++----------
 1 file changed, 4 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 9fd8d8a4..014cd152 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -148,7 +148,7 @@ public boolean cameraConnected(int index) {
   }
 
   private double getSeconds() {
-    return RobotController.getFPGATime();
+    return RobotController.getFPGATime() / 1_000_000;
   }
 
   private double minTagDistance(WallEyePoseResult result) {
@@ -185,7 +185,7 @@ private double averageTagDistance(WallEyePoseResult result) {
   }
 
   // Filters
-  private boolean camsAgreeWithWheels(Translation3d pose, WallEyeResult result) {
+  private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) {
 
     ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed();
     Pose2d curPose = driveSubsystem.getPoseMeters();
@@ -300,13 +300,11 @@ private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
   }
 
   private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) {
-    double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
-    double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
     // This filters out results by seeing if they are the height of the robot.
-    if (dist1 < dist2 && dist1 < VisionConstants.kRobotHeight && dist1 > 0) {
+    if (pose1.getZ() < VisionConstants.kRobotHeight && pose1.getZ() > 0) {
       return pose1;
     }
-    if (dist2 < dist1 && dist2 < VisionConstants.kRobotHeight && dist2 > 0) {
+    if (pose2.getZ() < VisionConstants.kRobotHeight && pose2.getZ() > 0) {
       return pose2;
     }
     // If we don't have enough data in the gyro buffer we default to returning a pose
@@ -420,10 +418,6 @@ public void periodic() {
         } else {
           logger.recordOutput("Vision/Rejected Cam", centerPose);
         }
-      } else if (res.getFirst() instanceof WallEyeTagResult) {
-        // I don't know what it does. I don't it does anything. But be careful about deletion.
-        WallEyeTagResult tags = (WallEyeTagResult) res.getFirst();
-        int idx = res.getSecond();
       }
     }
   }

From 1f148df8b796e9047c174fb6070290491d8e1b08 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 6 Feb 2025 19:43:42 -0500
Subject: [PATCH 11/21] Added a docs folder and the vision filter spreadsheets

---
 docs/camsAgreeWithWheels.xlsx | Bin 0 -> 8326 bytes
 docs/getStdFactor Upper.xlsx  | Bin 0 -> 7270 bytes
 docs/stdDevDecay.xlsx         | Bin 0 -> 6795 bytes
 3 files changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 docs/camsAgreeWithWheels.xlsx
 create mode 100644 docs/getStdFactor Upper.xlsx
 create mode 100644 docs/stdDevDecay.xlsx

diff --git a/docs/camsAgreeWithWheels.xlsx b/docs/camsAgreeWithWheels.xlsx
new file mode 100644
index 0000000000000000000000000000000000000000..4586661cf635b5274036258f85fb48c3fc3e0697
GIT binary patch
literal 8326
zcma)h1zc2J_b#17cL_-M5CRfX($d{s(w!pRIdpfI2-02BAV`aJx4@v}9rU|bU+?#S
z?_D#$IcLuA?C0#W_KIh(r6>yvhYN*-gakzpETIkchu}iaJ#5%aERCF8oPQnISv_oR
zjx=l)mVlV|@HZG*eT~RpR1-tO@RQ_i5`@LNz{{?zRiOq?mss{Xa|}TBYqm&e+;Y~`
zA7f6k`Gt1)TO~i@S$slCELM(MH?1L%qzD@=OO0K(pDSs#BDD$>k4eJ?M$F}IY@D@H
zf#TuSx6AwBo<>r-gh-zCq;iN!CBQpWUYR2IjN>Ug+8ip5DOI0V^=|_%z1v~?*zp{z
z6!ddXvMixKJ42Z&DZ-G`XgOih=F%I^Yr<RvB)5ocOneJ9ajj!^58o^bPF0c^TMTRf
zRw%WpKCNJPdrl+sT8pqG!SU65JPQ{1p?krCs-A4M(q(%~hvX1?@w@aeM;wAV_Rql6
ztl<w4p#JYj7My7{Y`9K(#bZfYoTq_PJOqr!OxzLPwIth$x{GXe=9M%BKN!qLF+Tfn
zn=xt|F$s}2(KSfz#-tXgzckkqHM$z6zwhd2<@{g@Tx~7#-eBgcd94aBYksTsRRUfe
ztNUq7aM%bQDL*t$Uw}@7?VuQ1in=0u8@m35z#;w$O@etoRBJ~PNH)+X1tpccTg&J}
zwhV33R~X7Pge`rp8or#OVG-93IH7u_6vZZe_6}{R)Xlr`A^G(-hBk$0;e+OCEAyvM
z+eRg8@y`w3wXG;1mfT!vNubkcp5;<I4`Jfx&iT-4O%gOXe8#nfL*kBQF?>HgVUr-x
z7Nz~^X={q+H$<d&*K{Or*q-%sX9eIyqrX<-N||DA`BX{eU%}>?%1d}Ly$O^<q=d={
zUpu7`6p%u+s#*xIb9Gl1L@q>bAU?-5e{Bt76BkKTag3=LD2O&h2LN2S;i_3p+f&Rh
z@jXXwPkyYB>^z`GH~e@YOQmiwd<e;G%-B!NpeIFBNz=*55%%k8Xs~Wpg&Qwzg3fS|
z4nzMaRM=9yx&3APC6Ce}HD0Vji%o^Ku;!ov*{9_3)v7Th@AN(CONZn7J!6ZLZ8=Hs
zvu`1`;@b)N&9bJ;Ea?g=ZX7NXHB2;KgJH2hB1h%#CPH!QoctRbEt#%KbB;YvJA-cX
znEFVPaZEaM#xyhFVHllRc#3Ebg+D*veefTiejco(<1m?ZKr$+_nV~-dDrYml%KyH&
zNsV)LYv(ixRwjd9DZ6LXY=2i+Hom4F*zj;0{T4=177>YlaVS_D779uX9STbEfAmel
zU%qMTWaMsTXW`8DpOc^dnU%O?w*&-)<QSe%>~_RsxF!(SM&^k`Lieg0GPzX^IWfOP
z6jR3DIg%v*+;YU4{)KQ;;T%U=QG3NzCLgv@o=|Vq1IwuI8E4Yu5{%G;Z<Dssroaqd
zlEgVoPE)V@R*I{}!tE|MzDS4L^Coz^HhX7LQtPICJ0tCCN^O#gQp7nsX|p}7K^YKr
zgk6|5OG%P7?^OnS0Ns;zzv9wK$_JI1)E4|Gd%zM?^&L{Jt};fQL$sw{ZY^rL5!#1)
z_{ldE^xc(?#b8wP&#E$pP?Q}{klVb?`YgmaoMhEcMD|sV=wG|vfKi%Q0Gl;&Jvkxc
zFvW}Ir~}H#Egw|jrBYrTEHBuK_esau<nj(}9NNfW({H^Sd-wH9cep0~*+7i4Z8R~9
zxoKLZG%yfcS3SZjtsxD@J}9EJ8LNFI*~}BjUHGN1k<tt#v2!W>)$_BxTtieHeSpr8
zJiL4zmmVwUtAV$Makf<Ru%ijF5ibPB>r2Yez#FHg@ujQ2CWVxWt2e$hm`(Z~M7LeR
z4=culs$U<@^lbAi(|d!kQ~@8dui5rKOb&eR$_05&eeNCoqNwocYeeLUZnOChL&}UR
zxg~owfy|rjux6o|3&-qNC>}&15&I`0M_<U-G7Rp#pPg(iZ^xZ-CqF<e1QF?({|~Z5
zXecOs_<yq)s=qA8z{$+!pLWCgcqMv57NrM>iLi>}5eoE9m<<l&I+6&oD|%umRg7Wt
zgj(tF2yM3~05yeE=z-_ejKJ9F6WsO9${-Ly*C1mF9lunQ+s+ZR<}1r*<Kv*9;HO-d
z#VwOj4DRc^KL$CB3xe2b-r_zjj|WH`3oK+_uoAxV1O%Rz+VRHC<MrzY*~9K94QaDb
z*0;dTvSi=a>a8e5$jdLIEnI8_DN=<biU43~kTW}Za&k9`;FL6sFLm#JI*l=BwZVJH
z3sOVW@c(ogcY7ymXG=3PNK*TG_~kS@3w9g4_;&$65PM5QzedoqX%NX1OP6o%l`wFb
z7w0TDfC$ZW{12Hguf(Let>V`MMEpdJ&nB;QL06>8mHj8=H_Mw<uVa?39XM%_6zFD*
z^z0i#OEUJ}-s}0QPY<2d^Vz7kUMnuYdaL?z@1*fTvu$qdYQtGAi?$`O_ssm-hp&Cq
zqHg25d9fy@)VB=%ddOt?P=oHy;+%`WgKOsXv_H6&$dK9mW(EvA@=oP>{^oOR?2_wc
zk$&fKMQ_<jVVPS+0{X-Ewl~l9y_Vj3EFU!<6u+2_rK_*L+R1i-JJ7nUt#0xAOp$xX
zN0)zB{^S1oo0j{J{DSY^txMUyE{<t-U+s!(#*BQAH|mfJ`bRIXCRV_$vKtsZ-`2q(
zt<eaP3-X|cw}My#RDF99RkymEp&zs}vcaCqsaVey;+Mqdwi>tHlsdEBmc%Zr(nt-i
z_<jf-SLogOa?U`aUU$w?$&SiR_n{j*XVY=YgI&ed#>`2A8}WnQ+{%*B{X9T;&UVDE
z>(ag2AhyKoVWRv2g(J(R{Nv2Q@rB~a{i2=x#5JdIK>EZ(<%*n147GjhiP!CWzA8Hc
zxw{uui@tUQ2jCR`<&D~hn<IRxw$%1LhuNKyt2>oR1#-HUj~nOZW!%%!DuN9dJ6Dyq
z?~YDsZoloV;oM&DMc(f$b>3f>-K`$beMj@vAANFj9KH!Mq{Jigo#>^BIF`NE3}NZ`
z&XW0}<C?{AKL0{z(kQZMB_Iu-GmAe#7>FE%E;@m{DGy^L;QwaaTo3`yNSK~Lz6}FN
zND_LIvO8V#sl<$;(oR=4H6;_wzK?&Tc>np{c@BMx?eyF}y@*UXe1d8pt`h(cl%jfO
zRY)7{`_kRTm(G+t-p&!R(=FVWG!6cWi)u}ELKG#HW1G4Sv94mxeV{JN1>?;XT<zJK
zF2Xyqewr+{wYJpx3%3PjYT6-SwxId@j{|jcbcdwL{Li!`ic_Kbg|QvZF`;wLp~AW>
zlT)ymImNifQ%ME{hUTRo1SeKUiq<oyU{FKt;cUK_pXsttY4-9cng^{hP5HEA;S)vC
zm{Snq%kV`}t#X<~@RE(qas)h!`oU{rML0rjQsT8T0_cdq4>koMZ=FW_P52N6{KC+P
zI#&y+kvYGV&@=<G;u1t~8V2|B!f;2Sa;I0B!C4cII1U6G=~5H%7j1M%eWM8jilU0V
z<K!Pk0>4V3V$Uvz+_PKm*aD|h6FeQ%PWd8mJ(J<drJ5PQhn58s?@b5kDF*^~WhB%A
zQ>ZkkQg9r4<@xHUOFIq7(Ovsvm&G`Cu&yk?XPfjLN1wJRjMczBZ%Nft5m5z&vGkUH
zJ_PQ}qQmp8w6931X=-?ewq&HpnoUfzJzJS>)^H5nevIO}(lP!MnMv8yZ#r}_uwhBY
zp_jxfPo7$B(A>t$^Ro$S;<kYI$x~bxJ@LYD^#+;_UK$(r@)@>=UFuWYb`*|0Kk9(;
zS7-DOzX01>U8zyb?2PCUj}Xo~pwF`z-w{ZY0QX{D7X7W}Brb0IhF#~`56vp_^x&V2
z%tPL*5Eea36IEjB$*bnXX<9&B1Foimf`gzYz-O(JZdbD-nY}WP6lm1f0t7V}4U_NG
zW5BB7QlLWFTVuf*NF8`VnGop=bcMKfg<&1^Ni9a;vF_9p6f6RnVni5fB8tXRx0xkz
z8lwEscKk?S4+bcX22dUZ=#B>JWyyu0=uK8pts@A@WEUPa3~L9dBnhl1p*m)uC_Ew*
zS0*rQ7jly13u$*Sr*|;MhUIim5i<306;5$8G2L8cwf&gt7O$(QjyV@F(3M<o+#7_3
ztZ4#oWg`B?1SrMN6jDl^=&0Xh$AQohRD)JI@`kYUnRJ{Z-xj%o$RZ6pbzz^^qLhXJ
zWDc<Y!n_xufXX%h`ZY&)?5j98fy!w(N|fVcH4{UUw`(Bs0sAr3!ZFmd*jS1q{U&^q
z{zBa2?b;}RHh?L=uh_L1!fd$kVmRg%FVd8H;qn=0K5qY4?D2O`QIt|{fxgyE6t+xZ
zZ`)w447x8#8C7pKs@4GpN))<3pDONGFHUjnh*l^VAgoG&t&6l)wFZb)q}T(IcNn~L
zQkZs9xSb|QpY_grGrTL}P6TTpAN}E*Vd#CFf0A7~<+A>*)n2j}y3{LVgt?l&u9WYW
ztP3<kQ7b6yW0=Mdx<SRknhh2rAS`$IzFh8m7}9UO-)r=h%y!>-_lQjMF~@@<9CN3Z
z(fc7Ip~ERJjV{!KtwK<4EbzFhdSp(Ph_F<x<4N8Q=sb;}cOAqa(!?t!Vm!>|pR7k=
zqIQ8#QdPFUGjF^6L_7KT&~o6IqR|7uzgVqF7>-vXs+qPh%AW;xh!moupSZ1vFc4g?
z3>qZdFF0$2VC)x?qgyWkD-pmkOry8X3e!(3uP_K@qna;d6t#myD4vEKhQd`MOasu|
zuJ*V#vZSx_HL$ze=9aC{X4fiWT9EoCDiP`cXyB_a%o}J(vu15?Z}vM%$0rTjcTGpK
z;mhgkh)1W00mtApv$ipcusyzYSz&-Tawk()FTq6bX%$>uJzQWT0jpL>4C9g5(s#$M
z5zdMN=Bk2KJ37q}T>O?ersB6SZkc+^DN>P?9y(K?qRXiLqM*xc(COsscU}V=$3Tzi
z=Pc5R6r#c<0U^b8Nz}=XRb5Mj$;=yeP$ChyD9+gFiRx;a+G<_P6m|6+ieydlu$1DK
zGTLT_R%TH4-s<mN97}Ma%~I)!T6#??_+>TRPduWFQ0vk23=3V6Hn1FYay(mJ1U5=C
zdbac_hXjFlaVZSXn^<ZE3i`Y#3rDE823xB*0Nu&b*-EtSSu#V@W+0Rxc;J9qmyM$i
zt*1oVMKwdwNFln2?6WypDj-!eOM*bbtB6T&YF}3^rh+N>2)xGr2n$>$qNsscl58b=
z(dp00fNUiWRUmQezUM$WhGqCWCJocdFHdo!-$g1E5|H=%>mOcY?Olom@^l61!8~n;
z6Sc7K88?p4?&tc>tvav8JXM#9Fq_!aGMJLxj~nm_a)smgV{;ErYYTEmO?t_G-r`YA
zg6o|Z@1?uzSjpXo&xm{vi>};tWA+3Y=9e9U0&?j;+?+XW@8?}u-OTSp2abMmQbhDr
zZdqg~s4ez?BV_;Kq<;{yU##>uE&KoCq-UR&GNSWWd$vX=e0mL@VQ|)w1oJ*;%G-no
zz8~HjLdPCoc5H{m$I*_itD`o`Ly~JRm*+AW=5fsQF(s;1>PZEypkmEPAyBzz+sJzn
z0Ez`dL_AapNTqBdjG1~$pf6MY=A`KIdXJnmcoO=gS(BC>kVx%tzu4XjvcP|-7hV%_
ziSx+It4a^#>L8?)@n1<P&M#i>Vrgb;#`g2~Cm$WtShD*<fq55j54+ypJ=f0As`NfX
zja*x8zhR%V_N=IT9){cMzJt)))<-tqoXX4qW`i@7ld~&sq@t4u9wR7{%fi~yKob)a
zf757q{n_oM^Mfa}e_U#C>ULD+8V=U!>+?PA8dFgYm_&XYrnZpq#C{Y?EP76S0#A|D
zYy2&NUS&DYDA5>lJX?u)6D`ifwZ!hloX!EE*nBRERsmk~prME$oQNV2E2th56PU>m
z9U6qmu)0nCEg3W=mKfsqO)NJF<K0@j-aVE;$C5Jo6QG`$>;uK3Za#wW7G~pIC$R=>
zkM|DEi;{JcbuS-czd&JecHJds{QIvJwFsre=*{*uf^(Pr9)T-lubZ0}wDo)&`@Lb?
z4)oZ38UwGi+k~(WY%CVY+E&&rdOa+7T8}CN=W>IJ4}JX_Y*$w7tX*KkovydcJR(I0
z#>;NgG8ZL@?i0@2VRqNckB0nw7^Q+>gIl*5ifIJ(u>|Ze`cla*!|D>-rx{t;Ibdgu
zES5_<52(rtVw7btzJ-HoL}js`%k{Q%?1#I(2I*42)?}OO?k7E9Uv0k48s3as6=g9w
zz%y1-`3QsCtQd+c?{rW|T4(R+!FaIug#nRSr*(_RElE^M%i<gy$9>_2kE-HVBW=Le
zqglz+Lx#ofZa~-5FkWQ*Jm1(zs04;cO$iZx^}Y3X{UW3=C>88v*YaD18A0!&63_zL
z5)rcfURKo<8P<T?K?JbDC_rqAC#QLKWXq#0ONr>b-}^H$+6lsN3+HL+c;*39IuwC5
zVcEeev^vo<WPC9{tI8*I{*^d)LOinF!C#ExRe^V95kj&wYktLPiwEXJ^wu91#=l%Q
zttZcZ93kB&NPCHeyctxKRc?<rv^&QWFe2@%nvWs<9qD*C{-KLe>HZ5V4q>q<_X0r0
z&gPgqvi+d^MFbU_z@1;7c)&r7vHmumZY;@j2~uq<Ax5s`o>q^yG1WKkRZgZdEh)~2
zbPn&*+wz9VG`3>Dx{d^2`lNVxivbH+28x<Dri#5j5|P6VIMs!>Jx`eNG2z)g(Kc;k
zx~#lXSQ>i&1M$((%nJOC=piMOs(&q+{HL7h3@K-tnX0%rJ=RSg%bB)uV={2;m_a#R
z?v_sOoL-tQIkZeCGY-4S+d`x*uv7O&i}~Mov}?T#>fX>Wm?;`vYa2Zc4^}DGUq0l6
zmofo|7wfmTztCG@Fm&8p&P4XU46Z~64+Bx_860c!2fb>Z#bObBY;Ak`497_;lZ!cF
z`$O(IDVAB<rS06&<vKP4p)<|ijJ`;bOF8fD`)8SzSp24dHK`L`5|-Ir*=v&+O5kwk
z{+6nqh|nfxL-~iC<(JasAFFQsLY>!buXhvtUYf!U0VO1H^a+dCXTQ(Q9%0>}J*p*r
zkN|525?vJjHU6Oeia*XSo;GGb_0g_2W|s-X^bP0;-14411WU@ny=+#zoO<#;$?KB1
zPP0~U5L_HrazZGUJX5@1E;9@*urlg%-V4}lZ2z>W#8=aW@6tX@AFnQ4b=Z+B)G#w+
zA>*^!uuWXayh|b2&awib653xIl@S5}vjp{Md96mHk?O?98k5qI=?Ix?(Bh329nPS+
z7korx+*;spqH1P20MFeuEP;i91Tx~2^mo{Xgj~_s-!k{o^*CA0X`SWIMfpNnL&e;K
z>&Y-)m(-*5rsA=jBiNk2?CK5)=*aQoxKh}Rw?0$}i8eN46%M4b>twQ2Guyy38i{@t
z;x6w<md758%rzVkNg-Rgm)#`F)eB`W%jp?~5t}V@u=ARS3)(Ek7I|s`1)d|TQ7p6`
zUy|}ul-t|d+1D!}Y%v(9DQEui?yIj!JD!8n#}&D<5;YS(U%x>AGO^uPZ!h)!hc*E&
z9S>++veJfcF|cfu`Luevm#o+{`V|*)IfXR)F^4WnN7&8T$~t%~7&-BIP7*cxbJq1M
z!q+a~DiyF1Y7$C8R3u4aSdAMZrC;7=%Eq!fxPWgKQf)43^^J5fV7Xu4((U@-8Lh4D
zu#bE;uS0<NJ=px=<et?T8%av?VcwZL`pDQ*UVVp1uo$}&2mTJfGcFypEV7qtGnHin
z?=*YoNC^Ue7ovX)`)D|Lizwpv5Tj8=__ySZ@yl=?lee+Gz4fCjWr1YvAjP8)g-QOG
zUjt2EGKERat0qCdi%+`dag5g_O*6Q@bR@*R%(uy}zRk3MvG8EGLY&358tBALXOjkk
zt8`FaP?7KG{EANk3L321T+|vI!h=~LP-rFJnULHWN1k1ZOJE$}E8q%40bs~#1okIP
zdG=DV-i3!O!lvZGgh@=RrFSUyCeiSSZtnnfwHTpm<aGh_9)TW{0{L%BRmmjMt;iiD
zkWT`bHk5fe<0gd_E#WUjGx#Z~wh4)jCsMrp2Z>o?WGXz4+G$@UBt$kYbZNdK){_gl
zs~gxoa~hdFZzF%=EZgV~X7Ro!4=+lr6)Ju9)+h3Zw(cuxP7KbwLCkpLh^5ey{_34j
zqr)j8_S9&U{ZfDGWD;FpHw!@BQZ!=tC))4{T=`p(YGmczuy{pA)Nf=5FvqCX;wvFB
za*+m)YalnAUnL1y16oL2`=2XX#J|?y7i;=+C4TazDv-$QULdA8WI4hX=H4eL%Y<jj
z(VH$6BVta?bJWsM$}L`lv$$9JFhvRU!yo!@A0{p9?ufn4{2AQ4nMxzD;k<Bcc68uR
zLQ`(&Fw^QATQHvBMWq_)tk~+;&h>FFxU!?7pK1A1MNBB0Z#YagBVh@ya8aVAYZj|Z
z*v7PoH#3e%rfm}`8kk13tHpsEU>pgl?&<is<SZNrMy-SuG#IJRn4NOvSwH~>p4=4y
zy~6qynk++cR);^(=)&qc)H?O+zwQNrqp{gM<k=hTy9Jn$&xxkk`Cnecrd5)wz9{Y=
zj3n3iWwpsf-75PKeKf=To5oNfcl=V<f2gGhdH@g;eiBmdGzTj?7)9m1W2d!2dy?BA
zMk!okRnc>Fq#TW!lJMi)x41IJ(-`Bzh=M71(8<seZ?BsedJfzs2!*Po%|iE{-mo9a
z^#cPN?@+3ps8CZEwsD}}3kmm4fzl@yL>x|*)5TOmBdHShO$o0P5q_{vTz%;zQGAnH
z`3c6S0M+nl|CBtV#ZJ-VscX)s0dhQpaB5k5Ac`jKJ+;u+umR@0t_sjw0nPTG72iVp
zJc`G1C;h)h_D5!^BW7>sVrJ)Jpz7&h=B)SVq#sme<snuf2<;x$CjLPOA1SX`?^Fv0
zdkZR1(5jNJVaxGtqXnp5N>v1=Y}xcsVB%(8zW7{kBLa=~eRqoS(~tSWHG#FhS`o4R
z=ezQ_iQOa)-6jlRDdQKd>8xO^vQ=}1qEhFWE1CG}T4)euzC|HQng8f?y(-oPN^+^)
zz=lpy;Kc4ZeIuzc_nSVHSylp!yPM<|T+D(xVvZZ-SF{l&WP$BtZq@gbjc40^qq~ML
zX<e3y%WVq}RG=lw)I7Gbq-R$<sYTVaqG@+sds#rnqCi!u?SUa%sS3L<)*T|lY?HvU
z#pjo3p#9;OPe9o#h%<4MZ99F4a$@?v%)BKS35Fw?V(ExsF@dt%6W*b{-Ta#{;aY0v
z#pu~Sh>8Z|>3rvO;bIYFlR|7>V@<;dl@CW9Z$sYk?N=PvaNPW?IExQX&5A<Qp9J@4
zZO|~dP`{g~f3|QQS;s%+zqM5VM)<R&|JdsJEt!yb073Y#ji0|!{;Y~WZnb{PIHXnZ
zC(196__K-h(>wkK{Bu|L&uM*vqzVY&A3M3<d$_+*{@fS+b6Uojf1~`G*l(==I;}tX
z-=oq07IUnhfPZhU{+`jFOzEEsHUaqyd9VMAH2#h7XUcqJRKH~#?-#=V;a7hT{WH=(
z=G5Op4$<*{l={Cj?B9d`40?~r;I|YI{T%#1q=vt7{){M(A@aA(Kw$p}n12ua(+~cs
d`!16I8h#XI;UTsH1qFb7SRlqMM)vsce*qFNN-Y2Y

literal 0
HcmV?d00001

diff --git a/docs/getStdFactor Upper.xlsx b/docs/getStdFactor Upper.xlsx
new file mode 100644
index 0000000000000000000000000000000000000000..fac1417913d357dd8ac113c03958e8a5e264d45d
GIT binary patch
literal 7270
zcma)B1z1#Tw}zp+ySrg%knZkAx^w96h9SfODM10H6hRuK1f)}tj-ez)L`0ChgP!M?
z<30bmcTMe?XTCLiuWzmQUF#ifH3URLI8;<rIKntZQ@A@u2s;jR<FR$J_V)Gp^}@>?
z=<ar4<nA#A#JxtkOu#+DatAHc`oA~Tzt>0?CG;xhu*%1I^b&s_q;ew>t}@xg+gDE~
zEdbfbZr$b{|1tJ?a1m1Hgr=&;hV11%2WG&PchDa}KH7d<7ZXnD9HS7QO9)IfD%@!5
zAz@);@~Iqo{uYrwna&rCa<r5~ub`YE>RE|n58qw&QP<1ullG`~=@;STQ8Je%Q>8>(
z_I1^#;<8XN9fa}(xB>ApL`^0WPh93gR;xK<z_)zjH;NlLUlVNoT3N9ZH{;@RbQMR3
zVuXOhGHu%QuYlMhOsX!q6lEFS>XS^4SV$@X<4(^!oru^^UCHxHsP~@vE4+K1mb_2u
zLVBjTYpO698W=rQF=A+h-oicQU}K1RS@>o25qJ)QwQl$+o@{g0-@)`jgPnlbvy>lE
zc^dud3Tkd2Lp+uU?laaiY?Tu|G$Be~Z0Ad~^znMD&P0ZJ23lJY!5Xb2H|T<pX<qgj
zn)srt?FC5tI{VPQDWc3TL|gBYSe#)j&eL-)A+td8Fr^NUIgM7~*NrrXst(4<bf_Vv
z?9)1He^#jTMtBE!iTPnBrRvO@p1y4WxrVsz>BN-#DKy{jbXNwux0PI4WzXNsQ@;94
zzqXLat<MNrPfROv=Keu&vnm^h#Ro2<EmSagTGKs)QIi1&nVt6otEt^7K1N4b#M(EW
znR5%56nAPw78j}yg-+ZhxhNhPL(|~*<~A7;HW^9;7vUccd_T_xM6lOvYu;QYCH8(y
z;N+w{NlO#E+B{>$ZL!#?H!T2lF25pd5^6i~*I!9~hSo@4hs))X>^q>&GgNUFJG@ZT
zV1<l{>BfY~UHu%Je+qbfAH!z&Cfr9R^!fWz+9$dJGZ|&9;r?0$UIcM<-kkid$CQYy
zZa89GB;dD7>>xVVujw1ry35+$7lWnm{GZGzu!pzpK)4W_F=@UkDb*M82fFfiQy&KF
zujJm-Xq@X^uAe6iORKb|@Deo=@Nuqe;@G;1ZQpO%-nPeIwtuH|spHUpy&aQe!rT9t
zq8=|SQ7eS{l83f4{MmV{YEsQHO<Cn2JbX#Z!&VZcdcVFK1Ovx?L`97T8L<H4oT!1B
z_IM_P<yYT}&z3eB8BWCoMV*fIsNgNjI|$tx!l+(cI~I*bboU%;tD&HVJr*ajLV$zQ
z#(_QlYlV{js!%&`>i}nvqYuwt2S2N{AafD42*iwkYISI})A6w2gNDmH?s#M>WX$_f
zujU%qhih=DFjZCZn`pwcv+CQOylvB^Mzt7aoPSKX5FH?jIBRG^RMhFMo4HC%wl_bH
zPmM;)kMZmA*{2Q+%7}c`4mD|HvI}X7Mm<g)o)}d*2+Ppf3*-{;Ay`C>c|R2GKy7Y3
zg+E6<X!_on5~?#BX;tzv?#oL$Sw-sB=+ayPx*NUexfc+#N=~SY^(7p9QBJ=}v#AjG
z#2L}_m%Jz`z(N2bm#DELht!(kgAzn!e8^%H?ZQpUrj4@d;qE$@a>NU&QL8$4Q2OGk
zv_%QzdLn=r_|epoH%qrx5J*LP@Tu308^lqnS%et1o~b;U%r+&>cJAJH5>BWZCfL>p
zR3<W(FkX5=Z!&3xd|+fiG^KD)Ys0oO;~Y2_<|Q^zKvvKFWQpl?>Kp04-p-Gx;uD=F
z!Ci5c4N?y#sZG1AF^A(I)txUm*tHc*5#2#x8QxY8mWAUv<eQd|c;&3XP8B;~dPYBp
ziQc#U8~a&Q^37A*9pjb$vqk`Cp1sH#Do39w&T_WDsgtc+q0z4M<Xlfui_H^-`{_o|
z7MsDk6)^37Ui_i`<kJxUcNJ1I-@>1msf3c9gI~~Oc`RA(?n9SD{1%2Ej~1=Db6&&W
z=8WvxtL@M5aBvn#|CTchf8>m%x4qlnGl%>3lhkoF^ll(7@+v`K0x%?FCN5FnKrt3n
zLT#m7isMGjsPpLnYo|LJBb#69Mp%7Xd~}4GaDB5f7DDoNfW3@GOu5;A`vBfp-AQn4
z3=$hhFK|-&W+H_xU|r<L0H4jXSYD<GLi!gEF%>_H&*y*RCRGo@jQLUy5=oyU>a~dV
zK-haUXv#%b{|0e}>&bPEIaCX*skw|b|7{~yn;||^1`~k^Ex%LvY2hXrqK=WxiP;t4
zHUu{ORhG4|3uJ_a;r}d~01t0hA18Zz7`gp?`BgSKt{%@s2;(EVt`ksC<QxOgJ1mPC
zpIK!5sGoKUaO~uZ6)1;ir`5PPoK1fh$>!gn!@=aaveDnq60YF@U71|o6yzRa9N(h;
z7A2fi$7;zkKecpmQJuiCrZL1mGPA}#zTR?5oxRf3>rp3XA)dV`QpL<JV>s6CZqTkC
zQ<&1|MV=LYjqkWO#>_MMT9+odtT2>q-E%o{(mWE*N`{;{H(&(FC7Hnhb%U^W3_lAa
zcbw)`!8`0$UCUnFED?89gCg%vGO~Z=gX)K8mX)c4Y0=l4SV58UIIO9Xa+&eH$JZZK
zOG0%BJ0a$}?ZRgFM)#K%N#?(0Zm8OR$g3caIM4e$*5#2Nniit1PBuV&Me+QGhX4^i
zR*@Ya+w~}@5;&PVIbQOf!`tOyZ>7o)V@`)39Sck*m&WQtVK2Zp;RJ_YdzSQ4L*9I0
z2wE^aiL>IxsILk-*21S58oN+g?(6(|*>e3QY?;8%mL?C$$X4;9c4*2VloAk};n#+C
z+PWtMy;7f<HZ+%$>^VPwa9J2rG45Q@F&Ic<xMpLxnV_PxZ8B1PKH4y3EEM#pP~XUs
z#EQ6${rL`|7k^6RRpO#G;-1nH7K5kMdN`@fIUY_A>cRtKYzAW~8^M&-d-U23?~!ER
zNbfK@t8P%pt@s|=Kv|a$jz9&jb<H=YNBH5nNRLY;B@+Ni=g@L}VLD!dl~e0<ovnmx
z_kq-NU(Y;%TMxZONCy4N2jMa`{cILzd{#ftife8V_r&3Yi0iht=5}ez$HJ$3LxShQ
zUI9@Z<+IL;Z~NikqM2-vr-y_-p=%gzTR-%hLm7UZ&WAkL&yjSZYJ<@)_{8F=Xy!Wk
z)%(hmtr-7K(XFlmW<FvsQNGEV#;C0=I6XA1qak`lD&FEqUdhQ0d9Az@$DV$-3Aeox
zg{iLoObnF~WDApbRH2C?v6v~VdZ}6kXNpZ+Twr2qSy@qAS@JK$UMNol_7xS$)0_jh
z;%Ppt6gwY1T#ZHOO655lqW`?Aro#9ZE-YnSP%>bh;_=yJvC|rv4S<0&^(xFBujz~O
zd!H`GOHvD`z_p)?-=87!j^N=>x=|%J_p-ogSM5nn-qYPsW9iW8C{ZX{W3!f@#>S}o
zPV9v-P8XN2$_v1e2`PsIUCNxe(@P+5zut7IM%lu2bNRqcU7S;nO*tQ6wQYu2phv3y
zmOg5`omd6?`NEr`K@U9FaCBC=bTnn9vqf7BO0Q+^8&5P&`tktI7kQQwT`SH6`((i4
znaGz+q`A`jGZX7EWoSSG=t1j<l026bB+ygfGUX6mW*VcMrwcX0wQb^{&8-;@v>&HS
zgokhc^h18sVsO!e>6zpY*s)5?6Q(@AA=N=|>s2(*02eMd52L5Lgx|V%C{qO)Y+KLI
zhh3b#`XSqN@zwfj`MVn9v1oT}t<3FV=yM?*Ss)z)fztuWo2yyzfCWa!P(U*eqaKFU
zdMlRkxi%bYBBAXSlpN?Q_L_7C>fVKEQ!GLX+470k2nEr8B1o%<v~vTzLXvAjQ6qr5
zcc)AH`O2f)u3CWzwkEO>wfSZw${VaU8?8iHJ2iaQ%K+uXeJ5HNTeeRYqwtJy0<~+a
zu1;7c$bsGzKt$%w#i%59@lsrPH`uQ?hdb#&!%76lab&hxvIMxFtS<&Oaj@ugjKA1G
zXz2(JU1{r@N^l^Sq3FnYmHZ{$PAkJEuX5YsC~R38<-BaXH{upyL-O8GQo;yElIY(M
z_AipTgRsA7<~Pp%zewij{o>=);??etBjcexmV!9^brf+Tq8vq=NWj{m-9c>pv1PAz
z1Y!cy)VexG>mpQ*_7|E0wnM^R`Jr}XH9Fln5GV%Tv@$Y7z!NvkU=qwy@pzd)eG*D#
zH?R#ycNzRi_Maq!t!aKsGI0~|hb_j;yqKAcp4SWQJrGA?dGn-d@Cm_f3|^J*Yt+G5
zhW%ezhTs>``a0RW+w=T<{z)*SMvI^&THLGXYlQXouGx0BR-M|%1~jGydyRYiHAf{~
za{wXd>kiTo_fWNB2L^jfzy^N;KmXhG;g_9cNI0>{0*<aumd3ca#GBSb>w=djJ~u(|
zQRz8xIa?`}YXo>-T#k3~tL<d@0GVP09BuJQnZ4+Ac&z-yBtbGc=foeydvrB|Qe@L;
zh};z)+M4iZu4Q&DJnih0dQdDt+bS;N5IYEtB><N|xFPkpxWIh2)Pz`Ew$&}huUU{u
z7=J{5l`DLN^Lnk_{2EWZV^J5I8fY%3c0;>hR*WqD5w~f!liUcQJ7k+lvTU7VJvfv+
zGDcc~*KE;;_<H4K4RSd-c8f=~#O#S!U<{PXrKM%w)I7YYHw2+=-<&73DdyC)O$vYC
z&2gTp4Z7~w6X+=1dQcfNTNqpVDLk^#9SQ}x`XVHGpMSIuOqT5%tGLX~Ur-{u&Nyxd
z?5w{y7>o>MSB^!9Yu#ciWs<PK69?hE%b_|+tjlblV&~%JLzuR9TrTh2XQ+6VrmKqc
zH3?EJtA;PC(bLYim*npPF=KQw=9%s4rQGLTZMiHM+Du=S<+9x;veD7&2N1SsC!lG1
z??0!k^9TxL-``zgL*cyN`cc^bk*tY{<MH=&p>M&&7<!S_DwaIm#+4l1RCv4rmMq<k
zV<k4C#WvPbWdJe*9TcS1TGum+64XREJ^U=c7nfSo5+NmJka?^{GBl6ff>)DNczyo+
zU|^$lwA`fd)0UaxkAW2hI%LPaAs@)G4v~l6@P8>E%irh7gClVzt=Lz`s*^oJBbJMF
zuB2v(swB9Q5?1SqTe5zr54@@XOQ|ufMV96+>^qRLx;D&@EuA;7XU+5vQ|^)E%HyGJ
z#+DSk@E{u8nH7#6RteWH#!)##{k-$=<}JI<^%6G$X{oHxJf<GV?XysF`~C|_FawYH
zRb-Ju^nRL+#TJp-0}4?^N>gVkc7d$!*1(9gs*75^!^wOn+T+3dpRV%SiiW6+K0a9S
z8;&~(%?=Ea13u^KD{0x7EDi1_qe1NRu1jhY&6p0g72Y{CwQJ)zsXWzM9IX9;a$9Kj
z&!VhZVN=j6?0*BBzjO*8m`-7Dr|0W^I||+E6n+n7)ZPJc<07JNtOu4gW@C0B4qaej
z1<!Wwcm4~AdCwh~UtWjk$&-L3I!=!qa)e!Oc9le|=}F|c8Xv2|#S8g!V>Sdzh?}jT
z(h3#v+hQ6g^Dx%Tl7JJ7<PoO3`Lx8?DjIgM+}C4^(j}Qang;}w^YHW1IO#8REDWX_
zzV#u&yN^V<dU4RAoIa`fF>U&A3VhQ3IJ>Ew(Ur;Zu)mW()>Aq6%<^mI6-6mia2idH
z;hdUiASc5poW`dMrtlGlR)b*d>Mdfc&mg4?XSI?N%5GoTqBYc1L2GHN`{Jvp2T$Ya
zatFJQ#8t9bc06t)_N)nD{sjg+-Tw+rSihjj$2Z8${%5G#^}GH9N>6$z6buOF)4aW=
z)qDmNhlp(36yf^-LN&)HR4L0RF82k&3+Glj!aX7Uz;r0KVg|`>8{q!L0rN9?Q~1={
zi)hKuFOqpO&)&NC2pXGA=mZDf4<8C6Z3x|Yj)_QC$>T&K8_|<PQXsJ8QJ76E!6njz
zkOvwMJ!JF<HM0|F9jP53@w9igX(nqlGRr?AWugpp>QRieo@i7Z@F{imA8j_y84(@j
zndDuPpgG5Qeh{SHFv*^xXIMXS|8*AuJPHMRuqcB4XXpUNY_k>_h>*hvo}AL_;!UB!
zBMr+~E@3{T713V5*ivza304g5XwI6_h;?*cOe9HZek|(U%$c$l2JJ0lyYw+br}Ur>
zijWVrfv=Msz!!4$h8NtSexYVuxybreU4nC}$=|cMwJ{3{jLScq(>0Fkh2Y<-XY37i
zMF)qGs${bn3TYj5={xWSLidlb^=cKF^AHWvE6ZE6)YdtcUq0ByJ`f{d_302%aWiQ%
zJk@bA{UlcQ=@^@xh?%0=TAQBQ9g8lqXmk1~oF!q9%9U&(prrMRWcOO|YI^Om+Vr01
z9@bHF699g9$7A2tC%KaR<he#falih$0@u#zS)-HcUd*J-{HEBh^rl<9?J?8g5ffJg
zY$`%QbyYi_+JhxX+P@BpFy-M^{=4OH8xIfH+lnfHsDWTZVZ7FanEXnNtvpAf@|^x7
z*nNTe?Hqy4nv&gPfu)X&bopYp;;PGh56Sr(5R|+?U^T{Dh{Y`zf>`OPJFlnN(YZoQ
z0f`-WwYgw2Fh~TLC(&x9*&bKg9z&a1OwVBNyZ1~W5gijp%_ycfV=}0Rf%_^cegPr7
z2#}~aWsujQ-Sdb^Saxe0XlB9=U#)3|ITsicI3ZpfR<2K_nCDF6sfczM&9R{?!k<1N
zt?h*LP4=-E9m5tW+2`@>;HUv|t~Aw`fd}o(>KPfyP4jP!)yd5@;;-uZc8<J<XO7!w
zsD0F$0={#FT+<|#WY$QP3r2({|1dREXXMA>zZ$@OXainMDC@1-PO$zoNyeL#ioRDK
z#h6857VhteS+|&qlJuTAX`E2=Ql<(`cPH_oHao^ws(rv`j4B0ae40kG<?SA53>Cjp
zg6#qGt;q9V1s?KWd+@7yy1NrU+ox9$8J8X)t^#a366a@YGjvsx@-<lP7E4iZC+GNT
znCLVX&c7E3t=_|xC9z1l>Ak#}aH_i^4?c=w3+Un~2je3K6S!^PM><T%zF@)4t#5jR
zLrs*DV|^d$Zc#J)j(^^d7X$mqB#Hq%uIsSjInjcOCjk|p!^$%*HB@v@d!x|8KCG0x
zMW$_O2W~e=|K14jB4xN{5fjjG^duQ^mew+2XFTHcF4W|LL|X<4y^QXWws17&8ccWo
z^aG0}v980Q)1rQ5H}-ofK2M+~Z<9xtI49aM*(9%+{5e8yC5^shY41QXjZp-`ZN#$3
zy7cy7@n`}3Tf{J6-}n`-zdB4A>}Vh^(gaM#aQLq4X`NE^nwQxPi@LB;j!wGF`DORP
zfo>{BcE*q6@Y2fcARC--*0daj1Kw6nM7v$&@U!3BB;YV~Ou1NUS*?2E{2JJJL<Vy}
zvQo`&@oi!vB^3iU#mlK3$@sjTrb-#4hI14>nloH7k$-THpDuM$Xouxgz6XRp!>~Ho
z`$F^h4M;nX-tX!AJ{ls+Bt|t4Ai6R0HKWu@VjpMG+n10CapU%%1^V2hi1t>-VEtDV
zzilw@%Xxr&?Loel`az!dKIXSY+Mutd3Cjuz_<+PVg@*gYs70mbUrYe_AK_vooGb4&
ze)NiHa)i_?>&pNtmhC=?k6+AbmL8jLfU%fsyRvQQ`-`QkV`{=pzz=$lcQgqzyC^)n
zY}vjm+eo(Naev3FSar}UDfdY`Ree}h0}r7qc6^Rr5j8SZuaEZ)J*ynlw{gEDW_;(E
zwTaS3DC`~j3^xhR)kW4DLfmI{<a`&p>dfFWs+jgs|ElYWrlYNQBRf{|%)ZOyFWjH+
z>%l8l7zBPSP?=fnWRx{9NoC&g>*0df$O82lw)zI$m0yCETsvfjcqV`q3!*1jki8*!
zYRD5P%5?ff+x9yY4LOT&PLVR446EUMxjdA_v>3Im@sNa`F0oBOl8M1_DfW|46m83~
zynDwiNpfJS2`Qf7(dHrK%A13Zi1^p{_FjIj=DYZ*Bq<C`&d9>T{|NCm+u#9&aKHZ~
zxhu}zHXnbtfBMVhPY-vcx?2(Vw-v+i0OsKzCEY*0+*P}7C$8T%14H(^UjB>7_ou_V
z!`<IcYaaPOWbnTvvp>Du9gqHgT4I=gdilq(@K1+#+rZnT|7}vRP6l?Jf6iEcI=I^v
z{e8pAaeq4aU%lg>9_~`+ZTIxs7Vv&~_)jnO=hC}Kf6J-A?FM!+u(Q95>whl33wpO?
z@Y`Zw12}B)f1!py{oF;ATZsH^4a9!~%s&_2t%JYEzL4Y};YV8y36?8xaG0<g7c7|>
I$Zp^L5BYK^d;kCd

literal 0
HcmV?d00001

diff --git a/docs/stdDevDecay.xlsx b/docs/stdDevDecay.xlsx
new file mode 100644
index 0000000000000000000000000000000000000000..c97415e25683c882f6e2c77b3902dfcd3917ca20
GIT binary patch
literal 6795
zcma)A1z1#Tx2C(hkweD-LnA01(g;XPmo$jf&`1mdBOr~G3=PtabT>*PNC^W7NFAmA
zLC^i=aL@7HHS^5gd!G5;*?WEKeOG8GBcqTYU|?V%P(GJ8K)7Kf@N+K*9t&GDx2Ntu
z4?u1&M~7YAd8c_1f{Wk_^wP09IqCVDc<l(|`j1dHh-t`OyG%PbOV`dQ$%(R~({;JH
zQjvKyDPodtB?LJzzfG8p2qWy=k=d&f)iF74@~M}&A~8f(lVy7H=wxGMlS}nQcbs_{
zlgcEO%J*j{y`ue!K{dvS$&xF)Z~FHI03`taP#ktiQ}+@jh!eN2f$4UbPjo?#32p7N
ztpG*(ouCvIYQL{27t6BPS@g(RU(*TyG<+G2V8;kXiz#JnBPkI8q<SStAZ<6PddnuT
z(RP7dt+x^8+8y$@<OfUIMTlP5T4_7AsIl<&*9Lo%;Pj%)i#@l<AFE%QSO#R|@C!VN
zNUm``pdI82A1eeEQ?grs7BOTuFtJ~2TA0Zz6%Cc5OtsJ02V_D+-s7{cWiF)YhzWuD
zecDCvky{=E>eXsz`ZLweGww1yRUgvgBsNTfG_~-lUJ-qbn3w0H!dRS;a3xE#5iH7O
z7BeIo3Suz{EXCfoaY&E7tgp4gtzqIw;-HxxKfklQOK9w7^_lAJo)~*g1>4h<P&w*X
zfT?3?*SujmjD8&3E7NLN%cIobFW5rjt|<hX6LwG5zM~67E7-(G9Wbc&<%W{VYQ%k8
zq@e}Y=&{os=MA1)7m6VUce=g!ka_2{1ysw0u(Lm^d#IZ`BrH?@QUN)d0!Bhx8PFFk
zG8V9RytH=|@*b%P6romqw?GCc&2A{`93EYan_K@~aka}9<ikf9-Q)bn%cjWww0_fl
zpQn6w0=@@sUq*`ekSOb-jJSJTfV$e!fXJNSeomtrx?L1@m$zfsTxDfCRjiK=hND>a
z?-=o<x2X%vG4JCNL%33&E@de@IJg-;boj(-SvhY`-oa8a!9(If2SUK4YI^ASUY@w#
z@++Qg)gs9Jy!Z&0(NmXO=DXc0OUn7)gG^uGf{xJ?5A9OS$Y$vv{;4b9A?%Y1EUtI>
z<7m$79d}E@rKlnF;R1F0NuKPb{GG+ZcL7Gi<1+HnLz$M(2+Ii6h6a)LoX6FUfcI&=
z_5-B6w}{)ZrwF${nv{9fT`+5($S+C=WPLon6lN1`i3P$qa2q|TLIf2Xw>`MFHqt!^
zx<9knh@75|vY_wsd?4OWNeN&oKU2uvAV=K2>KblE)KEsp5RcuL(?v!=z{W#B(D+wk
zQv4*QrJI?jos*3_&tE4$$eED<CnqmXSf=To>1OLo+UW<@UEE=4v}ibnL8UV_EQ1vU
zv^Yu+((CB`G}3CDY~3s%62t0DQg(jRcBHf@B6g}$ar!EMnAMlQnI&7>I_(?=%Z~7C
z0j;9h`W}e5>G~UZlCCM#1z{XU4~_#ASN+noU|x&@?)>vuA>(tw)^x@*6Ie5Z{RZRS
zG&}|NQf~(YrT-kJU?7e)mYP&ag<NGnaWGcq7E0W!;q?y<)EHXjGGa6&w><|x@#<`o
zKyLIz0f_3^*h{QFQqMy{BZ`}Ud8d(F-VMl<o4Ex8C<YYJ4x83FJ45DIBrlbiFYGqC
zOp8ie&ADD#8k*jfY&`1U;dbJLs%4`Dtt2Q`MzJ4CvWGkN9Ji7y`2m|Nc$Eb8?inl;
z@##;RqV3ukCmnX1Q}&KlJf`X}ja}bmJGCDB?$xLy-Sg(f$e?%5q2@b*S-$=i3La(&
zQl{aFrwv0Gv{&t-wb5(%B5FX6js0U4-}J_13A|W>eynogNE|dQw<TxU(6Xe*s1hy*
zSJM=CU!4Ykbdkx1-Dq4wNi9nAFO_`()j&bsf+KM|*r%Zd-qD<UeZ}QQ4IAG~<shf3
zopl_PBYqxT4H6%Y{9hYA08xa2VS9;t3-nuQCKv7=9|zOAvp?B!UHyod-4-ps9Yh2K
zW7OY84CAecF>$kU_<P)NUw;!ls*KgaOMtda>=n-I8$b0tLSR=u)G3e7R1u2rK*yxH
zvx~dg5saP0FL5QT@<Dufh>m1stt2##yseKTpH)n;{@LeUL_HN-!I6=;(C2>$977w&
zUa)(vh<xkgGtUhLG6#_SQS=f={)_l*`VlvUiZ@Qkeu0w+WQMf+ai}vg>{Y)3fT6k(
zWeV{6vch;t9jvCdh&y|<8mhq<mLP?L%#4}-PB=4bjS@vu*ZkP%0_mDG=KN(QmGB2-
zg1h1WB#o!Do4vcOl@&ar{W$z2O_E`34lhMmK>H=Sbo0z&q;zp0k>#pgrMqy-&ZpF%
z9LvB|GW^56IvOrT0Hj6hyWifSKcKa6B-WmY(?-@dbTSW%rLidMeXmr)mn7Jp$>?fB
z(b$kOGEWb?K~ZKzk;DSe2x>gOk0hH`vZPWHjA1aQ7OHs_NEEBKPnhSx+{JXjck*gS
z(1)oH8w_Qd$|_4USdl9<;Fz(DK*Q_oQ!w1gBucQ(!;q>-0-hZa&~B*$8nE0RtP!iY
zCaZ$dwUvf~%Cn1Ov1AU#WFjTHG*@26PPz^;#SV4*k|@mDe2N-OPl83hd6oy<5c*1&
zcChPCv;;0U6&k90n}Y4=`uP&;#g!IPBbuc0lZ{BYRwjOS&$CR%k$4Fsb<L3e`l=r9
zzLjf2j&9DFR)^4)d$c)7zi6ksl&)FsOeRrc%t&F?uRC0VuyL%%hB+X533%D}t{KHs
zb$IUcN*m>*#5fMCgk~f{5mDBdSobViZ$R!Hv9~rZ=4#&#g+ZUXd%F1CzDc`oTfN>U
z0BI_gt={#&H0vg@%O0d+^YwY=p&-Vu?5|Tn#@XiAEngCNN@`eP?qgCx%xT>$ZBf?X
z(O248l*fAyXi&SSdpz|O+8}Pdy&LRUh43~wamh_on3yaPpH3>Mh=61x>I82CS?+n8
z?Y&_2++hs=tH$8f@n<OSL=E)58R+$_ispkx?9iVG*60e86zjF;o3nPH_f~UzK%)Up
z!_D!K0nzF-i^xxu!s79RZ4c3^W9_HMCnaSt=a(IFlNNo1J!xrizgm>_H&{#PCO72f
zvy58tM0C$S;%U0D-`=!^FfWUukDj%6jM3i1t=`EZPjA_@8Z}spi9!1YB;WKfhh12j
z!OEaiPW>ZP0!Y+HE2v%y%E;Un(~;Orh0~T%xt;=oo0Y`q9t6QLprU<i>icXHT$qh9
z0%MtWP?2w08vJ?GCizsGv4X|8^a=LR7sjmAo3TR5G=3VjDHAf~ow!H&^Ub*QB+gJ2
z<6i2`(YglsszClX;g4BqHe>7M;DD<*nAB4Y=w~uc3@<)@c%L<hNBiRv4@0<8ihbJJ
zK6=nKxR1_JgGQ^nH(!9WJmB2Ud(}XFcelE}dV0N#m|(BtvYn$VW+v}-GYYEaYJkCF
zz%^5QRtdu`;py$q-=?>}#Oel9f6CP_zW%?&>R^06EjnkpV|{28)M+A!&tFCTTtt*J
zdkvMha$u_;k7#7kwFQ}s*dV&9ipeY+L$#$yO~7J6*fkwwNm-%UksP;#P540(jnVV9
zgPIRH4pcl$%1fJ^M$rLm&e@TVc${=wtnk!~uf^*57~)=o9t#jBfyw1^uB9{1hD_Et
zvK)L&eC>nMf^F3*xLR@iR;`GC3ieZ5D@QAyAD=&jY8YO6FWiBb-j~QLE$!1S>`j`L
zX*%=<I<Q(8f5kyw`wWth-DN9<uOmn~$C}Z~1ZkB&oS(l9GFbeM5*0r*O2Ee6)<lnh
zfNafdU`6o!*!{{IF$j|UJbB|q$tPmM{U?W8MCF#!d`Jmm#GK7xkqO;c41{d_WaQpb
z$=}J=#XBFWdcTm4p(k~ee`%r5pYSQ6eJ=A|k3?*az?~*>5$n)?a40c2FOEB|nt*^e
zojp1{lz@GCgXwEx+=NU*Sm0NgtXKGNKeZTN5{kFZKg6TsHI`Anx-(~#gC@C7P&fUK
zN*B4q_cQbT{1xgI9}rbwh@>3QXx^Rd@?&uYS^*VagLC=4>0>dkkR{qD4Gpsf#{PBP
zzR1nn#yp_9kW+(Z38HNWn_1fCr4^e_FB{>e-I9>$tWfBVe_*ZS(vp+?Q{+gu@9S1x
zQPMplh36^ha|)D~@rNx)n=3`T{ed73#ZctuO&jb`=6jC`#hviGl4*}4suEfzIRHRD
z<PT;xiv{nt84Gh`9xCB~jf^XoRwfcv?QG$LMLv5HXT<bGk7v5Qn`Rrh+;E;Tum)L{
z23Ty9nrmwHB9Sy`gk!3?ZNH(Za`yJ(*xp)TN9QtZS{HuyN?Kpv=I{g}bmT*Ztrb}Q
zz=WqmuY|LMmJsM^!rD<gl4mZOV{Rsqk3^}XiH^ElY5(WrJd6kgEuut^qI2~R_k8p6
z<7RQ^DKVY5GD;_C345MxgL!Msf@LOzGaIG`*S!ieG${|eecw~z?x77d^6wXnq;GSk
zB9Plt6mF~FR!JXVlF0<xmC&&Ul@MP@2rIWgUod;A&3jP@mQZH?6bMb3+qR};v#*&Q
zS@>SRk~q~nNCP8JktM`j3(d<YawhHHoE8oqeBiI0ga6=9j4zunui7{?FBiCpDWK9q
zvp8B#4qt?#TDFVsgBf|mF9NgWg12MLA8(Kv#ZrsP(-_!Ea0n!JG<gNYl$}*-?M<ZH
z-Z|_y+_^|?&K{uEU61|fG5Gu#l;q_r!}|u%lh?310rlynq(|v-tBPzEjsF0$5Z>G~
zux#c$E;&`7@2~uZeoZv1+#oYHxY(5bR&4%ia^2xgu9c<MQ@87a`nt)rf>fw=@e;_s
z3k=X}V8tV04W%)0l~m|vXpHCbB{l?4w>OEp2B>#nGCcn}JgEY-zIvq~VzI7WpyjDr
zjL@G1vE6)scCdaTg&38)+#@|xh+R21Pu<D&Ry$IRR1T_8I;$`glg7sYfbs~6>A%HG
z%?ES4$R>!x&T1F8c8J*btys3+tF^0ax%YJP7<TqkRrDr|+g8YzgN7fEyE-nbB8h+U
zwlA*;m)SZiHJp|lfGw_@j5iS?gG{M%9M&;k%Md{#&a0Mla}jmJvK!euw#u`UoOgt;
zJ@fvES#<_3F#5lhAl#o4<o?v#!Rm)gTD05$D?YEgspWKtXJg!4KPbER$q$fPG|MCQ
zAPH3*9@D;9Jbq#*2%h_Hnk?Mu%g+m0!jq3D-)crO9NT5dl{G+&t~?9A|D`C3C*jXF
z$4)^#{V`1+A4C5EKZ+XA<{KOo$`T%1a_NB1WbzDw1?Q|JvU>oLPUKXlQP3WfGswtN
zplPUbbjZcZ&b*$oR@W%~fP$IE%eGTK&}^($sm~p1|7^HkFL_9Gm}dgGaF6~w_M2UA
zjhYFL7g~?1hYa7g6C<KiWBG_8TYXvT!=A2Jr*tCWbVsD3aXo9y(s`w75(*IJL;Wb)
z?GXwUw;p4|<__knfCj8!0dY|8OY-B=Z6!?QH!x~!=vhKW9@#>?Td95DS9-mTv<9Ea
zR6jc726=#t04ZqNW$l79N>L|?+!{C;8F~fpzcbX1YWWh~t!C;5*<*qIC>|uSKN3>k
zZP&I2dM#}q;AvILv!tTvKuQXl5|vjt7mH)J@OH(B+1y)29ysVXKRVTXVz48YzjKJk
zLCQj1Zl>`Eog*$oVD{RF1Ao@=ep-9VInTVN3-YZ?!HW-{&dUw%y1;M`>g$jYw>F)(
zE!?9@sE*&L2ITZ=ugC#5Pyej7Rf2^~NX@Q^ZAq><gjpOi?;S7$0^n6NI)=US;Uo+m
zAThsfEbxB@$#t$acXqbFW==+&vJ<?C2vZ*ull>TCA<G$|IHUav{+^?2n;|y;q+ppQ
zu+SP0k<D?)DLYShzCU~Av_zF5upHth#Oja|hf?D5a8^sL_1#A@>bTIp(zQALzJ5}q
zS#tFz`p=^ZpGPpK<{|MMJ$G{jBCv4qm32e9<0rg38M!Ya!{(5avXLU>Cv{R=H9B80
z3rlZ&<~7piKrB}?!kO_3@fs7)@hi}#l~1*!cag{33+7yXD8dgJlhm+9J(5lnV_@8%
zp!_nL<P+3K1&C29_S$V>QHhU_s+(=oQ=u|e4ZEo7**tI?oH}f#r*l`X^E?6gUeZVA
zB~(Zh2nK+nz8M&)F!AH_U-S{YGzZUz=XaNV4maDGpadpIW5Ei7m=dXt{GZw2RLw`D
zM~<^Zj*_UIOO;_h+>Cgs!GZmib{pvncA4B#Sd40v$@Lzn4&;<7!1sU!E@S`R*--u5
zgP%px&7JsBCY8oXJ?Z2nkb`eW#O!os{6nS4bX7LXc_=!;#0*~rGlS~f_md2v<+}vZ
z<c}k-y3eo1Y^yG)d=7%xJ=-}8z(goM#15YgQTM`=&R7Xjs_Po@=}2EBn;9-SKCYPV
z;-B>ZV&fg?2Qh+2A6l=vj5S~o-dhr2z)jVIK9YBgX_RZ=7*t5vpwuw21h?owPHK@{
zDHtzV#RODsT*!y)B-M2}m=3tyveW=^!6x29#lfADk8Sh-{SdpIZ@8=xRjoSj9#?<d
z3O$J?;_*@g);YI}b73A*P5{MZzayuV&}-j^cK1cm>jofSdn}Q%T?+>HM+4IDJcbSb
z#ZPzrRY^+W1@jW1j=|er>yw8rW-qee0$Chz>9T5N7$o!UiaU09A4X#*#eX~Whn6IH
zo8upu-QmpYb2GIi-D;;ooIYv3hk&hV0AQ_TGwnw3sA1<3=}&f&mZ)zdG7ky7FYmb~
zUO;C<$>(M}31yTROqO@9kAISY_Kkb=bm1MfhF@~YI1(rq+jJMUPoLK4q~Z04N9K4B
zJ*i0~ld>}}mLAI`lf=h}9<J=R;<x~Dy_O#YedC;c=eq4>`>l#!7m|iD&Q4FQoSvF!
zd%IY<8()*OMq6189u@ZxJtLasY7EIRvZ2QN`bb3U2qE|EO77OKy9U(R#8oS5OCc35
zTJDICp3SI14~<vBxGa_JN#=j_=17)@RQT(IW4jMG)kqTBsa@JF*iRJA?>D7#pAZ%<
zTdU_4xW}9-y)3IhjAO{Ld4p9LG&EVQO?ZTrSm4yNYM2)?x_QV}M`JGJ*M&93O^$zY
zme@!_kXuE?clJ<)1)NVC(lY$4>~gH`V54hj(^Qt_=^|B;<C|?QMEOD;ul0-vQ_Js|
zq;>S8SvEa70deNiyxNQ#J^hY~#ZC+Mtx^L#W4wiPqQ|&#umM@RxYtYQA0T7RpS#di
zWgh!;iR9zQn+~SSq@qW}geY%}`i6J5i>)C=>gyar@m_<_HB3fQ?;f&7%7AIdBzSy=
z>j%(Eu6A1k!rtD66@Mw`JNq$tkn5Y6l7_qg70PwAAtI3={F<cS9OYdX8GpBbnycSN
zxH;On9{K&U6u2J15&m%icpK&BbmY3T`elP~Wxt8?FGHc*fH#}Czu(pf+7G}RjoYu5
z?KaBIHt6rS<%M+{<saL>+kiLAz3ZU=Wgs}+;m>)ym%0sbvnu-ghONUZNI1a%su6D^
z+)SC*)zdG#g6GL=g#Xr3x0l{j`s<wf%b1A%>yH1)u(ucA6us+Y@XHF|o`C=FzodrS
wI5!pLS|Weh9NZx{1oQU7n>_ft@4HF=QGPU(QQ@(IfPe#k0pP)`NPd0wA795qcmMzZ

literal 0
HcmV?d00001


From ce76c5d42c1483cb121eb10ca6e2983646a580e8 Mon Sep 17 00:00:00 2001
From: David Shen <dshen1248@gmail.com>
Date: Mon, 10 Feb 2025 18:38:30 -0500
Subject: [PATCH 12/21] merge main

---
 .../tagAlign/TagAlignSubsystem.java           | 32 +++++++++++--------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
index 9e05d3ae..3459e1ce 100644
--- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
@@ -56,6 +56,7 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu
     Logger.recordOutput("TagAlignSubsystem/TargetArea", -1);
     Logger.recordOutput("TagAlignSubsystem/TargetTag", -1);
     Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1);
+    Logger.recordOutput("TagAlignSubsystem/Hexant", -1);
   }
 
   // FIXME: uncomment when implemented
@@ -64,19 +65,24 @@ public int computeHexant(/* Alliance color */ ) {
         TagServoingConstants.kRedReefPose;
     double offset = Units.degreesToRadians(30);
 
-    return (((int)
-                (FastMath.normalizeZeroTwoPi(
-                            driveSubsystem
-                                    .getPoseMeters()
-                                    .getTranslation()
-                                    .minus(reefT)
-                                    .getAngle()
-                                    .getRadians()
-                                - offset)
-                        / Units.degreesToRadians(60)
-                    + offset))
-            + 3)
-        % 6;
+    int hexant =
+        (((int)
+                    (FastMath.normalizeZeroTwoPi(
+                                driveSubsystem
+                                        .getPoseMeters()
+                                        .getTranslation()
+                                        .minus(reefT)
+                                        .getAngle()
+                                        .getRadians()
+                                    - offset)
+                            / Units.degreesToRadians(60)
+                        + offset))
+                + 3)
+            % 6;
+
+    Logger.recordOutput("TagAlignSubsystem/Hexant", hexant);
+
+    return hexant;
   }
 
   // Red reef numbered like blue (red 0 is facing the same direction as blue 0)

From 5f49d99aa26ef9297676d173a4a92e51ba1c4c5d Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Tue, 11 Feb 2025 18:19:33 -0500
Subject: [PATCH 13/21] Adding more logging

---
 src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 1d5c89e7..a3b7720b 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -422,6 +422,7 @@ public void periodic() {
           // However we do have to be accepting the poses to use them
           if (visionUpdating) {
             driveSubsystem.addVisionMeasurement(robotPose, result.getTimeStamp(), stdMatrix);
+            logger.recordOutput("Vision/Vision Updating", visionUpdating);
           }
         } else {
           logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose);

From 7aa34ae8615c5d943e4d909f1df5a46b45659912 Mon Sep 17 00:00:00 2001
From: PotatoBoyH4 <isaac.hoekstra@gmail.com>
Date: Tue, 11 Feb 2025 21:11:59 -0500
Subject: [PATCH 14/21] Added debugging logging

---
 .../frc/robot/subsystems/drive/DriveSubsystem.java | 14 ++++++++++++++
 .../java/frc/robot/subsystems/drive/Swerve.java    |  3 +++
 .../java/frc/robot/subsystems/drive/SwerveIO.java  |  1 +
 .../robot/subsystems/vision/VisionSubsystem.java   | 13 +++++++++----
 4 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 7d2c3f66..c54822bb 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -2,6 +2,7 @@
 
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.controller.HolonomicDriveController;
 import edu.wpi.first.math.controller.PIDController;
@@ -168,10 +169,21 @@ public Trajectory<SwerveSample> getAutoTrajectory() {
   }
 
   public void addVisionMeasurement(Pose2d pose, double timestamp) {
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Time Since Vision Update", MathSharedStore.getTimestamp() - timestamp);
     io.addVisionMeasurement(pose, timestamp);
   }
 
   public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix<N3, N1> stdDevvs) {
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose);
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/stdDevvs", stdDevvs);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Time Since Vision Update", MathSharedStore.getTimestamp() - timestamp);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Vision Timestamp", timestamp);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Vision MathShared Time", MathSharedStore.getTimestamp());
     io.addVisionMeasurement(pose, timestamp, stdDevvs);
   }
 
@@ -334,6 +346,8 @@ public PIDController getomegaControllerNonProfiled() {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs(getName(), inputs);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "DriveSubsystem/Swerve Pose", inputs.swervePose);
     if (Math.abs(inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees())
         > DriveConstants.kGyroDifferentThreshold) {
       gyroDifferentCount++;
diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java
index ab676ece..67643493 100644
--- a/src/main/java/frc/robot/subsystems/drive/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java
@@ -113,6 +113,8 @@ public Swerve() {
             getSwerveModulePositions());
 
     swerveDrive.setOdometry(odometryStrategy);
+    org.littletonrobotics.junction.Logger.recordOutput(
+        "Swerve/OdometryStratgey Pose", odometryStrategy.getPoseMeters());
   }
 
   // Getters/Setter
@@ -249,6 +251,7 @@ public void updateInputs(SwerveIOInputs inputs) {
 
     inputs.odometryX = swerveDrive.getPoseMeters().getX();
     inputs.odometryY = swerveDrive.getPoseMeters().getY();
+    inputs.swervePose = odometryStrategy.getPoseMeters();
     inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees();
     inputs.gyroRotation2d = swerveDrive.getHeading();
     inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset);
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
index e4cef1fe..f8cd0b5c 100644
--- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
@@ -22,6 +22,7 @@ public interface SwerveIO {
   public static class SwerveIOInputs {
     public double odometryX = 0.0;
     public double odometryY = 0.0;
+    public Pose2d swervePose;
     public double odometryRotation2D = 0.0;
     public Rotation2d gyroRotation2d = new Rotation2d();
     public Rotation2d navxRotation2d = new Rotation2d();
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index a3b7720b..fb404b50 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -308,12 +309,13 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn
     double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
     double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
     // This filters out results by seeing if they are close to the right height
-    if (dist1 < dist2 && dist1 < 0.5) {
+    // If you use height ot gyro rotation should be determined experimentally
+    /*if (dist1 < dist2 && dist1 < 0.5) {
       return pose1;
     }
     if (dist2 < dist1 && dist2 < 0.5) {
       return pose2;
-    }
+    }*/
     // If we don't have enough data in the gyro buffer we default to returning a pose
     if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1;
 
@@ -369,7 +371,7 @@ public void periodic() {
         WallEyePoseResult result = (WallEyePoseResult) res.getFirst();
         int idx = res.getSecond();
 
-        logger.recordOutput("Vision", result.getTimeStamp());
+        logger.recordOutput("Vision/resultTime", result.getTimeStamp());
         for (int i = 0; i < 2; i++) {
           stdMatrix.set(
               i,
@@ -421,8 +423,11 @@ public void periodic() {
           logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose);
           // However we do have to be accepting the poses to use them
           if (visionUpdating) {
-            driveSubsystem.addVisionMeasurement(robotPose, result.getTimeStamp(), stdMatrix);
+            Matrix<N3, N1> testingMatrix = VecBuilder.fill(0.0001, 0.0001, 0.0001);
+            driveSubsystem.addVisionMeasurement(
+                robotPose, result.getTimeStamp() / 1_000_000, testingMatrix);
             logger.recordOutput("Vision/Vision Updating", visionUpdating);
+            logger.recordOutput("Vision/std", stdMatrix.get(0, 0));
           }
         } else {
           logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose);

From 00032f35dba7d32855487d00a029d1f31f22a37a Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 20 Feb 2025 18:15:51 -0500
Subject: [PATCH 15/21] calabrated cameras

---
 docs/proto/left_front_high_cal.json                          | 1 +
 docs/proto/rear_camera_cal.json                              | 1 +
 docs/proto/right_front_high_cal.json                         | 1 +
 .../java/frc/robot/subsystems/vision/VisionSubsystem.java    | 5 ++---
 4 files changed, 5 insertions(+), 3 deletions(-)
 create mode 100644 docs/proto/left_front_high_cal.json
 create mode 100644 docs/proto/rear_camera_cal.json
 create mode 100644 docs/proto/right_front_high_cal.json

diff --git a/docs/proto/left_front_high_cal.json b/docs/proto/left_front_high_cal.json
new file mode 100644
index 00000000..e578e9ea
--- /dev/null
+++ b/docs/proto/left_front_high_cal.json
@@ -0,0 +1 @@
+{"camPath": "pci-0000:00:14.0-usb-0:1:1.0-video-index0", "K": [[2034.590941433994, 0.0, 880.1279510276863], [0.0, 2032.7045196994395, 630.4273890059455], [0.0, 0.0, 1.0]], "dist": [[-0.5007775764633813, 0.38115685265411103, -0.0005456402979672171, 2.8719061218650646e-05, -0.28200501157108276]], "r": [[[2.972117756001366], [0.14461463521871412], [0.2051843867678562]], [[2.976346568914756], [0.23618136821777133], [-0.011004260754996312]], [[2.9313347527816878], [0.1245670188893653], [0.21736439731412535]], [[2.9831102269987464], [0.2774023825146539], [-0.10210655730656817]], [[-3.034651941328638], [-0.18310560010666244], [0.37206263315551363]], [[-3.0400913655858255], [-0.2027056414133386], [-0.21878484520999084]], [[2.9260831401685823], [-0.056334444902433466], [-0.17709704304964902]], [[3.116451676909422], [0.018001175565074086], [-0.2030843786420336]], [[3.1603646058730632], [0.04952560917584767], [-0.07893097318946792]], [[3.0693260232304747], [0.012003050980828116], [-0.33013014878847036]], [[-3.0988908047036463], [-0.0026349035125346086], [0.7540646514030214]], [[-3.0828616520098397], [-0.04177945037342782], [0.7570951437027]], [[2.92963618271969], [-0.20906643862935176], [-0.021118076060076325]], [[2.9081347820429255], [-0.054508342238345234], [-0.20775352571590078]], [[-2.9944502120555043], [-0.0974660007075925], [0.20859245408012414]], [[-2.991604225331016], [-0.16618430388057884], [0.36095871193177326]], [[-3.1647079218898515], [-0.1197256387978939], [-0.06681525741634772]], [[-3.133978086548387], [-0.0668229776382041], [-0.5884984995812175]], [[-3.1746634391767574], [-0.03373356757748671], [-0.26403033239869633]], [[-3.2294211903313927], [-0.05589308388977673], [-0.05061969574589772]], [[2.9752856911979833], [-0.021532459817112902], [-0.034441233724952364]], [[3.020475519377276], [0.00848291976311943], [-0.02951042971265873]], [[2.8420541455379937], [0.1775015031780268], [-0.26926731174912194]], [[3.0411301890777023], [0.23377827328239245], [-0.060432079825883264]], [[2.874567453068982], [0.14057586507265857], [-0.24402056321187568]], [[3.0421239254262695], [0.04614734562548441], [-0.39411635247706095]], [[3.154349286507335], [0.1753188430858127], [-0.316130569284971]], [[3.0206219840931907], [0.13587413253742384], [-0.017050719942209303]], [[2.876355652576727], [-0.02220815749331801], [0.5867225945279473]], [[2.8803774135519844], [0.02731741020076287], [0.5421230687579255]], [[2.8932209039944987], [-0.004624007904559221], [0.4444522457303273]], [[2.9595492673308748], [0.028329370238695136], [0.2994896807116636]], [[2.982237131338564], [0.07876501144483136], [0.09466031735732094]], [[2.981568712440064], [-0.018927347209293437], [-0.09897072580124078]], [[2.970670293725353], [-0.04862152524125523], [-0.2063035380621412]], [[2.880816581490945], [-0.13177412382821968], [-0.5637792475897628]], [[3.0144099669645756], [0.06621071086537973], [-0.230297199089011]], [[-3.155074389751369], [-0.12298218860380952], [-0.04645944554082293]], [[-3.1431917168336865], [-0.08836088956338276], [0.4206967650206471]], [[3.1495229625708663], [0.02095550128008066], [-0.6257296996551143]], [[2.9204163962502543], [0.01309510069097413], [-0.021470959809245564]], [[2.9221752280834212], [0.03766236221409568], [-0.651480128784315]], [[3.071326058612229], [0.009224864436857225], [0.12739790801807374]], [[2.950911307103778], [0.0013390198212066898], [-0.23544531312282188]], [[3.0466295576473863], [-0.026281541713614617], [-0.46976024183046594]], [[3.0766944726335015], [-0.3029528811867967], [0.017345076126612954]], [[2.8482598821772678], [-0.21260860984921248], [-0.8613863790420112]], [[2.8974045798970547], [-0.1433271480008438], [-0.5878901071773455]], [[2.8501408291432955], [-0.06951082636874774], [1.0374726721229455]], [[3.000078112605787], [0.03539296673546452], [0.7634352403806183]], [[-3.1750532263196645], [0.016416531351811894], [-0.5174337878306529]], [[2.856200383449051], [0.19713715199652704], [-0.4672569630872221]], [[2.8214183076009323], [-0.05720780792646842], [-0.9100853851595009]], [[3.029374357187629], [0.07417549748961703], [-1.1095993033772744]], [[-3.0886846670226964], [-0.10258972091970397], [0.5239281213633235]], [[-3.1145820975756946], [0.10149127847486653], [0.2611126866063612]], [[3.079135736615356], [0.20522394792623588], [-0.28087531419612616]], [[-3.18897885768096], [-0.14285095589888833], [0.1630180951106511]], [[2.903341700499522], [0.1272163618109912], [-0.5564430206105668]], [[3.0246667569907792], [0.0819801534311009], [-0.014000027671272121]], [[3.042382994582923], [-0.05147660807048582], [-0.2609246509134782]], [[2.9102671903622053], [-0.07152095458323127], [-0.24291229924889665]], [[2.931073390931532], [-0.12435304994943698], [0.05856818484624456]], [[2.8610704155950386], [-0.09391958142310317], [-0.17620783678679758]], [[3.0651305223747602], [0.15549124416862942], [0.37777411736342953]], [[3.1338755726753007], [0.037245200615811457], [-0.005630331789504855]], [[3.0195443012830085], [0.1299702427594392], [-0.10163906692961021]]], "t": [[[-15.650736854493887], [6.857778837599344], [48.349947504859855]], [[-8.963145207009878], [6.085300426994423], [49.72262077243192]], [[-20.08641853717427], [0.521253620895347], [48.59781936398458]], [[-6.453722785084623], [-0.5827928649768352], [49.24301661773489]], [[-6.281855327921188], [8.615084539536763], [52.55464194454267]], [[-18.85081484500004], [8.695364084304813], [47.66369326722435]], [[-15.765607168666666], [3.046700426563041], [47.93257956524282]], [[-13.36245182299302], [-0.062235632398827186], [48.13480954543554]], [[-18.829468978035123], [-0.3989734879366773], [47.865805448953246]], [[-10.08789005608475], [1.328458954057217], [49.17936463274357]], [[-6.992449373607359], [9.91524759457669], [53.51368287630119]], [[-7.1624409458373455], [10.806010649941507], [53.40062764613071]], [[-15.059198746102396], [11.481255809027674], [43.817949411970176]], [[-18.717957054866556], [3.1966421371936846], [46.85594249919136]], [[-18.611270176245544], [1.6126055370429144], [47.12695131730127]], [[-17.747915897386896], [6.071767311351638], [45.427001488861244]], [[-18.125060378387847], [10.005992371995882], [41.99602235488049]], [[-16.92131413478499], [11.265054125558194], [37.61750739322918]], [[-12.819801142117816], [11.46298422454891], [41.56995479249915]], [[-7.920763794708833], [10.352222797011878], [41.71513365057442]], [[-7.346825856250467], [2.558199867361427], [40.38011419784057]], [[-16.78696859181067], [1.945338134743699], [40.49679428400943]], [[-8.393641592490933], [2.078959790954781], [75.3842074754267]], [[-30.206172711677276], [2.0316159586058857], [72.1442238940005]], [[-7.225369573134432], [4.824453396537377], [67.49294542088528]], [[-0.420402559354698], [4.658001669352664], [63.75050849081555]], [[-6.354866609519846], [1.3280136301798813], [53.34989559635866]], [[-5.528381256729115], [-0.7931718717161952], [51.34362539235489]], [[-20.085416176322173], [0.5888710363981801], [46.520868222372684]], [[-19.345890664306737], [-0.5726899449772573], [46.78296193480784]], [[-21.773465406300414], [-0.18244735644177376], [46.49685502760371]], [[-22.160956036930607], [-1.818269951896011], [47.53361557823163]], [[-17.553246497806885], [-2.9140933657222114], [50.544172407276534]], [[-9.253775448674263], [-1.023075405254654], [51.14450074693739]], [[-6.092706186897927], [-0.5164866668622652], [50.56555017187598]], [[-6.910135699056776], [9.555348134967167], [52.9196998902071]], [[-16.21746585066221], [8.131313687007715], [51.416558737811876]], [[-20.617314521576162], [9.459738157703816], [44.105908925163504]], [[-4.765387855533012], [12.574426158500536], [54.185365335941256]], [[-8.211961261330442], [3.065440307607229], [57.2500100231776]], [[-10.233666475704165], [6.141928992236385], [88.64370437832548]], [[-25.334644439581474], [-0.9990450448463851], [93.6198651808735]], [[-27.097064550306122], [-0.14668818995942523], [85.8786216802266]], [[4.325561740167578], [7.794731384543963], [79.95653717189911]], [[1.8474374986682065], [-4.165117395845179], [86.06565109873223]], [[-24.8233831318882], [-0.3719449714386675], [75.45024123922931]], [[-8.099737015411032], [8.170831295695054], [76.3438321455779]], [[-7.609510422376858], [14.274072863261452], [73.98833506969655]], [[-24.44876295536298], [3.9978128965407485], [53.88066941041532]], [[-26.392281649296592], [-2.8438830215025495], [55.89196922666568]], [[-25.510699807996772], [13.693751791998594], [54.649549710713764]], [[-8.8095189144662], [1.1687595424314456], [52.10702417926316]], [[-4.673937035315859], [4.027697871341497], [59.44231545601794]], [[-8.780996099482838], [-0.8792527257577493], [74.58200870109113]], [[-17.88617387960956], [17.302497588157593], [99.55820561166256]], [[-39.46392780311513], [19.76930573862205], [94.1183673454856]], [[-1.0444980269008128], [7.836860254194232], [96.74692048917085]], [[2.4863765757054788], [21.265809786200965], [92.09457976399098]], [[3.4973771854522275], [-6.2796613320691845], [97.31178457875373]], [[-29.421272575574466], [-4.922554376629262], [85.19741843711749]], [[-15.442136777585938], [-3.805301363229867], [84.74404335948034]], [[-14.429326153096456], [8.057912642291827], [98.67973649204387]], [[-28.502837172606554], [8.640830469524762], [93.74397066111203]], [[-34.59878778234068], [9.812452174688147], [94.71762325157394]], [[-4.3648345358781375], [6.312010033352869], [92.12095005512731]], [[3.635043656796955], [7.1847893988502145], [92.79411601893081]], [[3.121313192267437], [0.29965290076028167], [90.80928777817076]]], "reproj": 0.07817802795697237, "resolution": [1600, 1200], "timestamp": "2025-02-17 19:04:50.283875"}
\ No newline at end of file
diff --git a/docs/proto/rear_camera_cal.json b/docs/proto/rear_camera_cal.json
new file mode 100644
index 00000000..a514b325
--- /dev/null
+++ b/docs/proto/rear_camera_cal.json
@@ -0,0 +1 @@
+{"camPath": "pci-0000:00:14.0-usb-0:1:1.0-video-index0", "K": [[2047.1009141217496, 0.0, 773.4246097465119], [0.0, 2045.5051381059277, 650.0368285341781], [0.0, 0.0, 1.0]], "dist": [[-0.4840171385752963, 0.2661609581256293, 0.0015279037363938586, -0.00038083121921188176, -0.03388670054693992]], "r": [[[-0.0863701224147809], [3.035186570605706], [0.2364467991583446]], [[-0.021885619222396357], [2.7697766719983488], [0.2283064987732237]], [[-0.13714149894228392], [2.8793520323442507], [0.20533449565962045]], [[-0.45759114305448223], [3.023373838604662], [-0.23289905350483775]], [[0.1866560337045247], [3.231262228557886], [-0.7301427430413656]], [[-0.19560765507704383], [3.251042174666278], [0.18870701765909534]], [[-0.022774611184455708], [3.2148204817820423], [0.3574247816155131]], [[0.11003486484605064], [3.237841558800873], [0.42352916629498655]], [[-0.09813441064578898], [3.1372736633714395], [0.3653785674773538]], [[0.14027304477571415], [2.9880230022794554], [0.27608871240674326]], [[-0.08279588168382077], [3.2618293511979264], [0.3942447626674682]], [[-0.09007040910452246], [3.074197936819594], [0.3142408032943701]], [[0.004919063694482478], [3.069223641616315], [0.34086945221303433]], [[0.1289928197025253], [-3.127090312817644], [-0.22975982480862905]], [[0.11622726916817629], [-3.129087010270174], [-0.3050659712749082]], [[0.08885685507214143], [-3.133172244878775], [-0.35005988531795934]], [[0.1274798081179404], [-2.9863934904819365], [-0.41569844301351655]], [[0.028722289076436203], [-2.9751817947748576], [-0.4131284463024813]], [[-0.009328516342440411], [3.2304294720023448], [0.3940193018669299]], [[0.09481380276898439], [3.181284429998413], [0.44395616150305767]], [[-0.05510254359321721], [3.0740371389983774], [0.4088171378364532]], [[-0.02645577545582269], [3.057977999820436], [0.14800502921435052]], [[-0.03281970082869163], [3.0726777435861234], [0.3040986806916974]], [[0.01172978883734732], [-3.1890180150312406], [-0.3570837766080885]], [[-0.007525380292261355], [-3.1964665500546863], [-0.2243824749647905]], [[-0.05953596726173425], [-3.076242767746407], [-0.3760005489804243]], [[-0.19769295648093765], [3.043845667872465], [0.44979153300935665]], [[0.08198602253891142], [-3.0781764424341866], [-0.4669869449363024]], [[0.040885860283610805], [-3.215037805368028], [-0.29489582718996266]], [[0.17464403398502384], [-3.2617430293504186], [-0.17129676612132336]], [[-0.1325185966293476], [2.864454818087606], [0.147675607125941]], [[-0.13253232189539738], [2.9616189432884616], [0.13423077297113328]], [[-0.08039637150419161], [2.9865812530610802], [0.1709439014671603]], [[-0.0953037231941376], [3.068861769628926], [0.15939687045633757]], [[0.007354070902647101], [-2.4719702883922503], [-0.275443699736768]], [[0.07669367685926197], [2.3016893633945745], [0.34819023808075883]], [[-0.10517010992932119], [2.896338810055806], [0.2767746693361172]], [[0.09787053772143628], [-2.4414591416223206], [-0.1503817786657787]], [[0.028404789846106754], [-2.470041586352834], [-0.2672689619591153]], [[0.1641582141897427], [2.475811422118353], [0.31318194396025345]], [[0.09323936993627299], [2.465180440388126], [0.4532471369291233]], [[0.22611620272274435], [2.7465242592665327], [0.3027706632048043]], [[0.04240785452423825], [2.374878375617106], [0.30393773318851347]], [[0.07586879107570839], [2.8619787925396802], [-0.5169707800106885]], [[0.11473553739616736], [2.930586376886018], [-0.8965741796242038]], [[-0.061030081464936436], [-3.268201418696969], [-0.4647972721035759]], [[0.031090976832200457], [-3.217197775323007], [-0.5351181506340187]], [[0.07760883697708343], [-2.965135288134805], [-0.4553347208361466]], [[0.04115491834818518], [2.9984936777192415], [0.5498497687481599]], [[0.03864743284369913], [2.987099572064882], [0.49700920660594883]], [[0.3566302617453058], [3.0673714652447326], [0.3251390830778495]], [[0.3446460566694717], [3.0172084397720886], [0.07039002948415127]], [[0.05131172523880532], [2.736728384401224], [-0.09408258036776523]], [[0.17118429189256], [2.5644982628021715], [-0.020142841530069646]], [[0.27228353489391144], [2.5041149502650186], [-0.02716617644516712]], [[-0.27723408435258545], [-3.2130809385357963], [0.5561219254171655]], [[-0.07933721816551445], [2.8794247483221374], [0.5473529088471765]], [[-0.060620858845273656], [2.8972628854616445], [0.2511787862094709]], [[0.2972983707886323], [2.814959397700737], [-0.18909855862699082]], [[-0.05823217538926851], [-3.2803100218625323], [-0.4084476832990864]], [[0.00435331578065358], [2.7591901586913923], [-0.182049132078234]], [[-0.15481733245122695], [2.8354999825845137], [0.709362455705872]], [[0.029249496963081066], [3.0094368323103597], [0.14018881211992235]], [[0.0007996169348159774], [3.0171148975580873], [0.1661796237879701]], [[-0.040261077250760044], [3.1976294212342204], [0.1750133367967596]], [[-0.008619934850856416], [3.0050866533585925], [0.2870251256938012]], [[-0.0806263966277856], [-3.366138960186526], [-0.4236103782561241]], [[0.2957513562699151], [2.8803034103502045], [-0.593164017079708]], [[0.2939607165289015], [2.3161338438963606], [0.4268480951121812]], [[0.1569735648648808], [-2.617057755510327], [-0.38923821084531385]], [[-0.3016520242605625], [3.2046924840441497], [0.5287040697707057]], [[-0.09186790220507059], [3.075541641994474], [0.3788978591831696]], [[-0.8781771836979836], [2.7332152348663303], [0.3580616387760539]], [[1.0738336615011428], [3.029146456084457], [0.5686371972225847]], [[-0.8771549428781893], [-3.0338226839689617], [-0.6349077485541286]], [[1.2549982694380246], [2.5861644517723033], [0.6492306849702273]], [[-1.9130571082379086], [-2.0475459300544143], [-0.17035803323951923]], [[-0.6857906529552971], [-2.7841674134465384], [-0.265229020935039]], [[0.0050360663938963345], [3.0701187375032286], [0.5072628603563256]], [[0.9317872960689443], [-2.9547737741011444], [-0.42830987849486757]], [[0.07909550100803779], [-2.4690804603349963], [-0.41938099533709916]], [[0.2519731006634613], [2.608500047505476], [0.44155827525892366]], [[0.3314572453845145], [2.793186723994225], [0.18890332545496588]], [[0.12096801343157873], [-2.5828596089879503], [-0.44836067973376587]], [[0.0012415000774946533], [-3.198197425004392], [-0.26168775055443755]], [[0.030848358633007233], [-2.458771034652745], [-0.3322757718543232]], [[-0.07104552289033333], [3.0178916489696332], [0.5519141982176218]], [[-0.43622404715657653], [-2.7426793096248834], [-0.19613270536331526]], [[-0.8412252148325976], [2.7198536250421665], [0.2210015548318672]], [[-0.8037085752574878], [2.6492971667268175], [0.3058726787701514]]], "t": [[[13.323312154279089], [-10.042069280884277], [53.323196849006074]], [[0.7751714440598305], [-10.765744064154417], [60.65285061618623]], [[3.239972383770365], [-3.576543475709984], [60.442565616563556]], [[22.72752573232495], [-4.979845155454233], [66.09616182850814]], [[20.66953941261572], [-13.354281246080982], [64.46307863750509]], [[22.450977371809028], [-2.9210445498207567], [61.53932898405472]], [[17.788457671113218], [-1.7928341618769588], [61.471121233754545]], [[24.82365918064157], [-10.360831614553854], [63.63980097579028]], [[26.12709955522726], [1.1350278894547854], [63.38182358158413]], [[25.903605384867856], [-0.46535839675985385], [68.76179696976685]], [[23.366780094657834], [-4.40956319717228], [57.923431973174054]], [[10.040779156509405], [-13.51708654875411], [45.0692939932895]], [[17.545468842813364], [-14.144437342986373], [46.95634343383758]], [[5.357934439124253], [-14.784781737599276], [47.57245088151832]], [[4.887373825722519], [-13.692376636541804], [48.08562386000808]], [[3.093634057813879], [-13.490769217593929], [46.83409697600755]], [[7.711026585482903], [-12.927367733549316], [45.3979049236329]], [[12.785021467095289], [-14.084075462935319], [45.778490940167245]], [[20.57259545177022], [-14.871331952712977], [46.1656492827402]], [[19.120619892168737], [-11.220881272418348], [53.16071585778473]], [[21.556084543472398], [-5.3295073367335], [55.298639986635045]], [[22.792296393642076], [-0.5077629402762289], [55.53591804732441]], [[15.519754569823062], [-0.5298107729229434], [53.649936572399994]], [[3.3612588190820283], [-1.1026063568422173], [50.324183505941804]], [[0.627273418160914], [-7.946803714779384], [55.9172753212724]], [[-2.278442983087149], [-12.237840150490488], [74.58207034115624]], [[9.60573112670132], [-4.280999064549282], [56.50418641087598]], [[-0.11275910252759178], [-7.128659833246716], [55.331944980333645]], [[0.5488480757812004], [-13.024874908798102], [56.1833685405388]], [[1.3384972075197594], [-15.522565350161843], [59.10640660258637]], [[1.54340043143556], [0.42393704346809513], [61.521928780493944]], [[9.131785403889669], [-0.3764771020285489], [61.58762275858321]], [[15.78527032379509], [0.5926548063395475], [60.10601930648435]], [[21.600056950233228], [1.092408734459844], [58.71089220581107]], [[12.47115023117911], [-8.868503584882212], [57.39815197623647]], [[1.1047002140101887], [-8.932046371027011], [61.65699153956785]], [[18.869483380233167], [-4.52302310920874], [85.20720931296863]], [[21.109437477140414], [-12.623318558897559], [51.93182400208613]], [[21.35000693219333], [-16.885225665008765], [50.877363291922734]], [[3.7959653815854337], [-20.301766671490835], [64.67210397737577]], [[4.4654917346798095], [-11.874796583446663], [66.09513690819699]], [[22.844219943495325], [-9.28017028103934], [59.79123170345728]], [[9.300596590865663], [-6.409939749995828], [77.04487972121454]], [[3.4836084547775], [-4.962995930388924], [55.04848568458167]], [[17.665350462384062], [-7.734572420642511], [59.81984973766614]], [[-17.640574452876688], [-24.24027158389322], [105.87124421142849]], [[-5.969499985401794], [-22.78736742601784], [104.78851982228291]], [[17.597841010021664], [-20.93307190582558], [97.33635209800899]], [[21.895745120043916], [-21.253055955338667], [97.61036069405714]], [[33.06637759335781], [-20.489906796656555], [94.89271856878307]], [[30.40544809662242], [-26.621521636644232], [93.99331121158484]], [[33.877152355793235], [-15.283278056872147], [99.63169813238913]], [[14.934431681537932], [-3.6986652036865038], [102.17349881975116]], [[14.338048734582204], [-5.6848413291714035], [104.04440899204248]], [[14.487408216809609], [-7.372644574530333], [104.56098395498412]], [[-7.995185930415387], [-3.2342899441920223], [97.23917314916359]], [[7.94610722783565], [-15.714701149234942], [92.97175441644818]], [[28.021626789119786], [-11.985729978640055], [101.75887934648244]], [[33.63349235526131], [-11.945372249896334], [104.17099159611453]], [[-19.403767724198882], [-24.763656329177042], [107.9020022229244]], [[-2.964717420052391], [-29.64679531838568], [109.64874604706718]], [[26.295140168267153], [-13.51350004422035], [98.91531080585587]], [[22.903696245703344], [8.073469152466169], [108.84979434643978]], [[31.34851346795196], [8.428521965755118], [109.0959528760275]], [[38.29420399422579], [9.346405389583417], [105.78543751506012]], [[4.243887544909235], [7.9834276902852475], [108.05710252930176]], [[-7.49762097150184], [6.290146998987512], [106.98825743396495]], [[-18.65886446065867], [5.6350820224705895], [105.43944571128887]], [[-9.338811913967254], [-9.813636187000858], [101.2337101269918]], [[15.050200168220478], [-14.838640653206829], [79.68134838581307]], [[36.885763180747354], [-7.946095305782554], [84.52221518546911]], [[15.843564734979722], [-14.318966343351518], [71.33194087539471]], [[15.102849972662106], [-7.750645188045616], [86.73383256626767]], [[24.155443795055838], [-19.0385360490709], [72.59563993441647]], [[-3.686525587563905], [-21.44458865081142], [68.28343903430273]], [[-3.727075929586441], [-16.003368178670478], [71.10912679934239]], [[8.100795355460587], [-16.837016549472942], [68.91740243567102]], [[11.443517436107607], [-11.458083457642973], [68.75974131471143]], [[14.54821557735335], [-11.087047591751105], [49.615740607685105]], [[8.945608272421968], [-1.9607622457452685], [52.922831696264666]], [[22.69110939047353], [-18.536734058044875], [85.08294462037236]], [[27.01104890875035], [-11.0221085557174], [89.80061329232088]], [[21.91976785071343], [1.399296545888009], [88.72394769153188]], [[1.511753712262244], [-12.45624699055754], [67.19668284763263]], [[-3.4476033964244004], [-0.4627179682480428], [76.73743324217345]], [[21.456260369361903], [-12.875977568877932], [59.13232331034403]], [[12.87170206608341], [-23.741525577933025], [100.09574787792164]], [[14.442901652535081], [-26.723816168507085], [79.74205804234988]], [[21.55661578512654], [-4.776075916627366], [86.03982520399752]], [[25.471338921477958], [1.6478950651637814], [89.53017231054773]]], "reproj": 0.12688228841148913, "resolution": [1600, 1200], "timestamp": "2025-02-17 18:46:47.692716"}
\ No newline at end of file
diff --git a/docs/proto/right_front_high_cal.json b/docs/proto/right_front_high_cal.json
new file mode 100644
index 00000000..2b5c3768
--- /dev/null
+++ b/docs/proto/right_front_high_cal.json
@@ -0,0 +1 @@
+{"camPath": "pci-0000:00:14.0-usb-0:1:1.0-video-index0", "K": [[2032.1882419306107, 0.0, 804.4939962394094], [0.0, 2030.6970117117796, 647.5345935217175], [0.0, 0.0, 1.0]], "dist": [[-0.4830072739533347, 0.26588374860363284, -0.0014829031974709483, -0.0001996425603932771, -0.055057461223499755]], "r": [[[-3.0930214318632583], [-0.057296044155564725], [0.020079810907895165]], [[-2.9606702977076713], [0.014507341805404169], [0.847086516184715]], [[3.1012542354592667], [-0.1570095931072575], [-0.32909842301075115]], [[3.05163695501229], [-0.0026624947493445288], [-0.21801100942504023]], [[2.924590170563744], [0.07298631720177717], [-0.12766723795593174]], [[2.901859873496244], [-0.07559990241547668], [-0.19089291361511518]], [[3.007927287345037], [-0.14999809836201108], [-0.011789822233182397]], [[2.9354844222476406], [-0.06039062018828159], [-0.3401310971234539]], [[3.052222757689557], [-0.06924753230173097], [-0.33352408042957266]], [[3.0524328813985226], [-0.19283842117742234], [-0.2838334510168063]], [[3.1377915343671674], [0.05641721148104438], [-0.2294359716391342]], [[3.092388475785859], [0.023397395432350192], [-0.49490080632116845]], [[3.033943513005759], [0.11549672158566096], [-0.4733889308538441]], [[3.066232301896761], [-0.022624021346004882], [-0.023577077778777062]], [[3.0421531961936537], [0.009154469387156099], [-0.013249387641212636]], [[2.9088591848519205], [-0.11691846821005678], [0.14136114781855802]], [[2.9357137207159494], [-0.08714304116638989], [0.07703089128528975]], [[2.9238230835168912], [-0.14911183825497085], [0.1052230785301802]], [[2.957718441436724], [-0.23731544590084572], [0.04725935385275927]], [[-3.2393601686595286], [0.17884303129220233], [0.08388750217165641]], [[2.889202002319232], [-0.23077168429949338], [-0.18645151236984833]], [[2.8949691116457], [-0.07669229837613585], [-0.1754832856099144]], [[3.02085473589064], [-0.12426956946239702], [-0.2635518002949415]], [[2.843609027863969], [-0.1357363122219755], [0.2367177180198798]], [[2.884562205956063], [-0.09293987044241979], [0.20065018300067794]], [[3.008023970888268], [-0.06392651905355283], [-0.00758846281493229]], [[2.989978340606269], [0.05063337040268764], [-0.19686369001708307]], [[3.031835524786863], [-0.01861639572479011], [-0.2801886508998091]], [[3.023136937162395], [-0.00032462696390040687], [-0.5261706366502296]], [[3.0400992887020535], [-0.011360452473848523], [-0.49488012553503363]], [[3.0548966779826476], [-0.036941870245199133], [-0.41783363842764354]], [[2.98661828054258], [-0.010782916079750881], [-0.06100888393309246]], [[2.9994843204041812], [0.02679850054591895], [-0.16275967167145308]], [[-3.056581771251195], [-0.10102528491622838], [0.3416773440131372]], [[2.728513148966791], [0.06094930889141968], [0.07698343101432167]], [[2.9569691883029288], [0.09474863834279128], [0.044853178806785694]], [[2.9570564310116896], [0.1781384915153472], [-0.043399094579290906]], [[2.9609309225391485], [-0.0208181972656203], [-0.02990496663542331]], [[2.915912192599624], [0.044196371099587815], [-0.16284643058397458]], [[2.946216523655372], [0.030402209102058826], [-0.14834535135249424]], [[2.946446464807985], [0.007840331837336827], [-0.35072508522466805]], [[2.8026137117820533], [0.028429345009661284], [-0.48758032949429536]], [[2.9053016425379505], [-0.023906223376883737], [-0.20759745129439588]], [[-3.1167191539180816], [0.07807069182231816], [-0.5909242972846065]], [[-3.1170859694885196], [-0.025905273141796036], [-0.6160740082733568]], [[2.9531293294423477], [-0.12037359110465368], [-0.007836630484487593]], [[2.8999222119608703], [-0.1825513306969949], [-0.1517724644790825]], [[2.9234436791568803], [-0.10727992337476394], [-0.07009965327412604]], [[2.845335550887965], [-0.11027270427147175], [0.85030801910682]], [[2.9258504599779687], [-0.13562961452821193], [-0.5579126852961405]], [[2.9511658867220065], [-0.20116635842741895], [0.5180316201670115]], [[2.7420594860760277], [-0.1512916753239637], [1.328142712489899]], [[2.833879058793612], [-0.11761130576733471], [0.7407935186097946]], [[2.8150052133664283], [-0.25644804637778656], [0.36866307745304877]], [[2.793400187315943], [-0.20598089429380753], [0.6155800936524911]], [[2.954788370139054], [0.026906107869822168], [-0.6486264984589758]], [[2.8948739704368216], [-0.09528369829956429], [-0.6915023150255635]], [[3.0350719107554207], [-0.14867434451470543], [0.40007861786177723]], [[-2.892463394921752], [-0.17069941183932155], [-1.3339847516454981]], [[2.7836270312983924], [0.011193216453735944], [-0.9686228635236468]], [[2.806846759308544], [0.10738372796635876], [0.9268930556603684]]], "t": [[[-8.213244996060114], [6.523592466478713], [39.53371434208401]], [[-11.463871566364357], [7.217383279051943], [45.13133128212191]], [[-10.01162588472026], [8.400006083152038], [38.699666594520785]], [[-9.403277409501307], [5.689819762527628], [39.28241416696817]], [[-6.557180001983456], [6.886153330497469], [65.46977374967352]], [[-21.98971904671578], [7.568966695716252], [62.086375529537364]], [[-20.35934301601565], [3.3215937948243193], [59.855102128534945]], [[-3.180149843429044], [2.704205984420306], [60.62139631362196]], [[-5.826542545212883], [-0.35517175374127324], [61.60573265705676]], [[-5.695507446947208], [0.9113198700143575], [61.4405387722682]], [[-9.555888586357915], [-2.0431516591128913], [60.91395124137467]], [[0.4154736422295592], [-1.8895000502214494], [59.445663991568296]], [[-5.849306606310612], [-3.173377826744456], [58.89383823341902]], [[-18.56397244656208], [-1.193603492071035], [53.31426310838562]], [[-20.591166596819093], [-1.3623856370133614], [52.64177161630148]], [[-20.323808847477785], [4.85279987773712], [49.335037372800485]], [[-19.92415802547869], [10.958564030824954], [49.560470004643285]], [[-19.683663465717288], [14.512840546257292], [48.57161994933358]], [[-18.47061223677007], [14.711246797431887], [49.681750252001365]], [[-9.590193003003101], [14.413400301144739], [51.002960297749084]], [[-2.775342491268333], [10.698151562520065], [47.6304740659804]], [[-3.550513569443053], [12.08536034620109], [47.17916906439629]], [[-4.6328576764890785], [0.8645328333924511], [50.30866460136573]], [[-15.949789662001665], [9.963063525514094], [84.04653118151155]], [[-27.821611206006338], [10.025806310115568], [81.6620202640263]], [[-29.532164767750942], [2.540537229442574], [84.99538330314152]], [[-11.160977788690063], [0.8811987467596352], [81.99778366522402]], [[3.8950934371224055], [0.7540078989470562], [77.00529793854378]], [[3.062980943935862], [-1.795674946874096], [68.96570766527378]], [[0.9084053585308309], [-2.3126079373310544], [68.7565770663574]], [[-11.322778443428641], [-1.4631842613552017], [68.58617413797332]], [[-21.139538202914956], [1.9933108799056471], [63.99847636697116]], [[-23.76899881501207], [-1.1715658463879635], [65.42924918646003]], [[-19.982927187530304], [10.990031578730335], [64.83553551856653]], [[-18.6318355801416], [14.854636777605876], [58.01170392112516]], [[-22.87540992063406], [14.814939843900392], [58.36495124922197]], [[-15.535501229686469], [13.435803213873344], [58.80261515191293]], [[-13.572155331476672], [14.791900637464035], [58.468720947740906]], [[-3.2369373110369537], [14.288793151051928], [58.94890954262475]], [[-2.768836749123345], [15.596108078721937], [69.59383483192006]], [[3.5555412460046907], [15.874878252085704], [70.39528282300684]], [[2.8587020192716426], [9.072713364252659], [71.76575282397336]], [[-17.352802129796423], [10.818030709705582], [67.48784724699304]], [[-24.08916273325243], [10.652286538081466], [58.34855661319745]], [[-12.523257235199646], [10.360487092794383], [54.6435272412304]], [[-4.839126641353199], [8.584328931931479], [47.039107927797126]], [[-5.54114423796796], [4.521790140672614], [51.211604495050665]], [[-4.061260291810564], [4.642313940204542], [50.194794741175144]], [[-14.556555539014076], [1.8904247433111139], [35.32182887706984]], [[-5.1128254169775635], [6.607040428450149], [46.48017086707289]], [[-14.27289920175703], [7.309860558133728], [41.55442140043684]], [[-14.668305974077024], [7.28306135454236], [38.04847725727089]], [[-4.975955582700343], [1.5242590958171327], [39.418499880643054]], [[-3.9631254394199797], [10.195706907608013], [40.22639268838452]], [[-2.5047019610261017], [9.76524668880873], [37.85043181160343]], [[-15.191653077316905], [5.443777010721254], [46.05183619472418]], [[-15.472232809057932], [9.727686706795032], [45.26773442012783]], [[-24.252437808702503], [12.425235434294528], [75.47477788022637]], [[-30.4075650791391], [13.23291680216156], [74.62471713324085]], [[0.5063311224897741], [0.7062799652644354], [84.62486116157099]], [[-10.60559831008954], [8.847198146848617], [60.168384640721214]]], "reproj": 0.05696388080234508, "resolution": [1600, 1200], "timestamp": "2025-02-17 19:23:06.523660"}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index fb404b50..9c30916f 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -384,7 +384,7 @@ public void periodic() {
         Rotation3d cameraRotation = new Rotation3d();
 
         if (result.getNumTags() > 1) {
-          // If there our more then one tag in an image we can get pose possible pose
+          // If there our more then one tag in an image we get one possible pose
           cameraPose = result.getCameraPose();
 
           robotTranslation =
@@ -424,8 +424,7 @@ public void periodic() {
           // However we do have to be accepting the poses to use them
           if (visionUpdating) {
             Matrix<N3, N1> testingMatrix = VecBuilder.fill(0.0001, 0.0001, 0.0001);
-            driveSubsystem.addVisionMeasurement(
-                robotPose, result.getTimeStamp() / 1_000_000, testingMatrix);
+            driveSubsystem.addVisionMeasurement(robotPose, result.getTimeStamp() / 1_000_000);
             logger.recordOutput("Vision/Vision Updating", visionUpdating);
             logger.recordOutput("Vision/std", stdMatrix.get(0, 0));
           }

From b40493386692f5fb8cde16292d00a6b071639c45 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 20 Feb 2025 19:00:36 -0500
Subject: [PATCH 16/21] Calabrated left servo camera

---
 docs/proto/left_servo_cal.json | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 docs/proto/left_servo_cal.json

diff --git a/docs/proto/left_servo_cal.json b/docs/proto/left_servo_cal.json
new file mode 100644
index 00000000..34129dc8
--- /dev/null
+++ b/docs/proto/left_servo_cal.json
@@ -0,0 +1 @@
+{"camPath": "pci-0000:00:14.0-usb-0:1:1.0-video-index0", "K": [[908.667484904358, 0.0, 788.9292626711189], [0.0, 907.7612107273449, 600.5577142958227], [0.0, 0.0, 1.0]], "dist": [[0.05860039588562758, -0.09926103008871819, -0.00029923249702769485, -0.0007110488647505913, 0.033992311176953754]], "r": [[[0.01708959763952807], [3.1066764619498173], [-0.28373965552941005]], [[-0.012101146688600843], [-3.0769036480309007], [0.1216148216260416]], [[0.014298101471690092], [3.0776941938220714], [-0.07165568951139943]], [[0.01556324543337185], [3.0975414201949243], [-0.16453244575464923]], [[0.008259897892452716], [3.0808505408538136], [-0.02918813541874695]], [[0.24511806711339595], [3.122534009026846], [-0.09676697125103548]], [[0.3048333378002546], [3.114372452040457], [-0.13789898306554826]], [[0.08606722295612744], [-3.134223333446275], [-0.07551105871905583]], [[-0.07794414768500942], [-3.13485567060615], [-0.056656374750222296]], [[-0.1405116296930595], [-3.077898763381189], [0.1036183804013022]], [[-0.11826276931641563], [-3.047097169760671], [0.05706613298131639]], [[-0.048934217440727595], [-3.111391936618772], [0.08460261221500895]], [[-0.24419735259882486], [-2.874442521218518], [-0.061063382175233695]], [[0.06547745733255005], [-3.0179621830786862], [0.09692226857792774]], [[0.008077908935851696], [3.073827923132955], [-0.052866236390603075]], [[-0.009230697428827775], [-3.0646754793538316], [0.14397619155453378]], [[-0.007244128592945403], [-3.0185819926610393], [0.13877482543209707]], [[-0.011605047868139842], [-3.044470544389751], [0.1105777305684842]], [[0.0028299164504184933], [2.9496228686795605], [-0.09091495789687122]], [[-0.0005993163273238655], [3.0160980762566174], [-0.03403058533439466]], [[0.2002582000150506], [-3.0564285522230548], [0.17152626087857448]], [[-0.14997306690848664], [-3.0705098423824118], [0.1247086864557807]], [[0.18082066211161066], [3.047678338369997], [-0.20312518659930995]], [[-0.07652385501553692], [3.1304444193970604], [-0.13156832190077838]], [[0.09968658949666151], [-3.0722239007565273], [0.2879206690886277]], [[0.057716120150259544], [-2.9874304360607273], [0.07249088504275197]], [[0.08352004672365419], [-2.8623949150525334], [0.02228882782737979]], [[0.20709261996043693], [-2.906528296445862], [0.04320421416792938]], [[-0.019998223544194536], [-3.0892512079745185], [0.13478711653466682]], [[-0.003007448980000043], [3.1238894457349087], [0.014548338888883506]], [[-0.00998234862355455], [-3.1513266428542392], [0.2015415264894833]], [[-0.03634496451392213], [-2.9521829921804756], [0.0931692716350578]], [[0.10597080378986488], [3.1197415181127877], [0.007477781476248389]], [[-0.08973942914247096], [-3.0432474713882693], [0.0026454821871140055]], [[0.06581650589211797], [2.8945769086962034], [-0.3023056534776636]], [[-0.06669042620831586], [-2.9855834047840646], [0.19231717652129962]], [[-0.0824449040117578], [-3.060021919155849], [-0.08053250070709317]], [[0.12296378334721336], [3.0099616037843315], [-0.13437718509261917]], [[0.12936232020155689], [-3.1229390876357965], [0.04960103746620885]], [[0.17360143867276723], [-3.1236916217960347], [-0.13469753194680695]], [[0.09051359900318924], [-3.0775489116324812], [-0.016131620088832174]], [[0.14996417947069446], [3.0814341303345985], [-0.0771846126993105]], [[0.27268644802257064], [2.803239099347612], [-0.2450457233258271]], [[-0.05087325313355352], [2.935150241323143], [0.06821166239592341]], [[0.14765356666155977], [2.990640710765321], [0.014830438832482987]], [[-0.08533953854239335], [-2.9060826281352616], [0.12895231200432897]], [[0.03917451994842752], [-2.9621914558487266], [-6.8279816892886625e-06]], [[-0.21971995763378654], [-2.8937376879227936], [0.11579227670927804]], [[0.061686078362830084], [-2.8300055375514583], [0.26622394084447154]], [[0.029782790804300303], [-3.0623157949144684], [0.1394461001442277]], [[0.02434214824507887], [-2.973158965441152], [0.34451597051136634]], [[-0.012111919587846335], [3.0906137999211283], [-0.3003991919146121]], [[0.06423283419312927], [-3.0160952146889164], [0.2995864976733363]], [[0.04959158730991548], [3.1437153548462655], [-0.34827048495061974]], [[0.01264626729180785], [-2.9802501928576115], [0.32471896336142325]], [[0.07484902306768355], [-2.943744645071098], [0.2568461014790691]], [[0.04290718766461925], [2.9743536335794105], [-0.5394401716237774]], [[0.019078237208410415], [3.048532416762353], [-0.2861642695444935]], [[0.011811879609799358], [3.1385266584750156], [-0.32269484988563785]], [[0.022843875833779587], [3.079016605007116], [-0.1455268486567791]], [[0.013014674964648311], [3.092515851170159], [-0.29268239906074967]], [[0.008697865786941629], [3.0894592919747463], [-0.3574362543144296]], [[0.01347033041341125], [3.0662236363927122], [-0.34579077291987503]], [[0.0964633623875692], [-3.0498860886765073], [0.5357760408720327]], [[0.1221886267598363], [-3.0156456383556534], [0.3520981774763901]], [[0.05985604731294491], [-3.033687593884458], [0.24054468751910174]], [[-0.14110633693310434], [-3.121700783568948], [0.26632418030759425]]], "t": [[[11.255746250523966], [-6.228528990898213], [56.154432651357155]], [[2.0309980861250274], [-6.421246035215185], [53.12347607354304]], [[0.24708666868375728], [-6.635277888851757], [39.33236960889902]], [[14.650248074450657], [-6.851020732809918], [34.49910484488878]], [[25.18796279464805], [-6.963343721071457], [33.94730207836129]], [[2.045868182660418], [-15.3367717661473], [38.140573697789485]], [[-5.271735585351751], [-15.675529837483628], [37.31423155927086]], [[14.60847647709072], [-12.744907554982127], [36.01288946695217]], [[24.364792565411214], [-12.381458804856464], [36.3067469240715]], [[21.998309127022974], [-14.522013339414544], [36.544532337853184]], [[21.301291528379355], [-21.939216978238136], [36.14448285231962]], [[14.944709335174457], [-18.73660546822585], [35.93230407509502]], [[3.4052492751333827], [-18.110925469755774], [31.681561565351767]], [[-2.552837853691686], [-16.94036197875656], [32.466544289937914]], [[12.938434190370995], [-6.10661175334262], [66.04957906465293]], [[-2.011792806279801], [-6.0340488379469255], [63.305816194858544]], [[-13.338752383858616], [-5.972523178050512], [62.57054737387954]], [[-21.853871269081914], [-5.917103674783385], [62.94857054885268]], [[25.09526659965938], [-6.228254316551207], [62.54135482324705]], [[33.55657402891661], [-6.320612432048879], [61.31406784111346]], [[30.31942811805321], [-19.84009945777402], [63.61425208840028]], [[23.724930693011633], [-22.444465821465513], [65.63241299175455]], [[30.370662484535547], [-22.606269249697256], [63.200866958664825]], [[43.02677033996023], [-22.071283118778894], [54.637429919023546]], [[25.072605803179027], [-22.01269002384017], [54.83781763867724]], [[11.617396675379846], [-22.062053717291242], [52.49897009430294]], [[2.6381235631798963], [-22.342857525444536], [53.121402302485]], [[9.961839300699372], [-20.09309514518559], [51.94605151841695]], [[-9.623083590537616], [-22.247464975065526], [53.19546169820401]], [[27.553471950826818], [-6.369505900445974], [63.39324929634588]], [[-21.04512684431377], [-5.990254109889054], [58.124566433479174]], [[10.77313968369506], [-7.504412202393244], [23.53807379756939]], [[16.92806730584147], [-18.53886656642328], [33.630573018395346]], [[19.974011922907977], [-20.585661391925537], [32.637123154592196]], [[7.5680410265188875], [-7.142057006440287], [28.627833626593393]], [[29.357110315386926], [-30.958332839109744], [87.40828372675352]], [[16.551145100699213], [-30.937905969522205], [64.46843941393806]], [[4.680328896624454], [-31.23895224826385], [67.33773507909748]], [[5.907686708031065], [-35.535502344165785], [65.33338252098268]], [[-10.092149860565426], [-34.864274796749186], [64.59353079071737]], [[-25.336791004164336], [-34.03874088416885], [62.012106242976735]], [[-21.03653171185467], [-29.58060300362278], [66.98102085354758]], [[-15.237949487591306], [-29.092836137992], [72.57520939186406]], [[-11.198629284605406], [-29.170291001191504], [69.34264084568063]], [[-26.238349903682685], [-31.397344411558315], [65.79321680747394]], [[25.32774763111954], [-29.577901157341625], [61.36853914626458]], [[14.535713565688763], [-28.76881043485494], [61.46261377495044]], [[36.27687452876517], [-31.37039329455821], [63.289094790754376]], [[33.37673734039401], [-15.436013835698153], [56.85362085009581]], [[41.56711574511026], [-16.129712925495543], [58.48824794221913]], [[21.031283268687293], [-13.75447906561627], [63.36903938080954]], [[0.5880940981222447], [-13.275732668959737], [61.66861259306706]], [[-2.5297182368590705], [-13.250747403912515], [58.467875621222134]], [[-10.719375432658744], [-13.683276496425558], [60.47328353871305]], [[-16.067864987667626], [-13.359564496426982], [59.602969317025575]], [[-21.76195801816655], [-12.151915484974639], [58.079113212292185]], [[-7.410775743503267], [-5.353827833645429], [54.860649129955185]], [[-13.200145821492955], [-6.020902026435965], [51.661566273467315]], [[3.7886406503795156], [-6.176895133975448], [50.614728106450094]], [[14.797927614035768], [-6.41795208132265], [54.97222591922958]], [[24.079043558750307], [-6.279104189194652], [56.21323465820686]], [[34.3086435705577], [-6.195086341056795], [56.75089365311261]], [[37.66435214036778], [-6.248961974005903], [56.57714702093321]], [[19.290432543617207], [-13.751877034985329], [65.34272150733918]], [[10.935230597783589], [-16.533663472551503], [66.15087645210647]], [[4.263481186670238], [-16.286655338966504], [67.28196508004051]], [[19.065944109537185], [-16.891839039907893], [68.4629080707033]]], "reproj": 0.15416319457295144, "resolution": [1600, 1300], "timestamp": "2025-02-20 18:53:43.383888"}
\ No newline at end of file

From b150bcfe431dc865bd21c233335468d7407a1a70 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 20 Feb 2025 19:52:22 -0500
Subject: [PATCH 17/21] Slaved over camera positions

---
 .../java/frc/robot/constants/VisionConstants.java  | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 9f61663d..83b7ab69 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -85,26 +85,26 @@ public final class VisionConstants {
   public static final int kCam2Idx = 1;
   public static final int kCam3Idx = 0;
   public static final int kCam4Idx = 1;
-  public static final int kCam5Idx = 0;
+  public static final int kCam5Idx = 0;  //?
 
   public static final double kLoopTime = 0.02;
   public static final int kCircularBufferSize = 1000;
   // Poses
   public static final Pose3d kCam1Pose =
-      new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d());
+      new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d());
   public static final Pose3d kCam2Pose =
       new Pose3d(
-          new Translation3d(-0.21, -0.31, 0.44),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0)));
+          new Translation3d(0.236, -0.108, 0.932),
+          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-45)));
   public static final Pose3d kCam3Pose =
-      new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d());
+      new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d());
   public static final Pose3d kCam4Pose =
       new Pose3d(
-          new Translation3d(-0.22, -0.335, 0.50),
+          new Translation3d(0.236, -0.038, 0.932),
           new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
   public static final Pose3d kCam5Pose =
       new Pose3d(
-          new Translation3d(-0.22, -0.335, 0.50),
+          new Translation3d(-0.229, -0.073, 0.934),
           new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
   // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
   // in the form [x, y, theta], with units in radians.

From 16a1c6cf05dd2cf6767c82194ccac961b332427e Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 20 Feb 2025 20:55:51 -0500
Subject: [PATCH 18/21] stuff

---
 src/main/java/frc/robot/constants/VisionConstants.java   | 2 +-
 .../frc/robot/subsystems/vision/VisionSubsystem.java     | 9 +++++----
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 83b7ab69..81dd2d64 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -85,7 +85,7 @@ public final class VisionConstants {
   public static final int kCam2Idx = 1;
   public static final int kCam3Idx = 0;
   public static final int kCam4Idx = 1;
-  public static final int kCam5Idx = 0;  //?
+  public static final int kCam5Idx = 0; // ?
 
   public static final double kLoopTime = 0.02;
   public static final int kCircularBufferSize = 1000;
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index 9c30916f..f8942534 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
-import frc.robot.subsystems.drive.Swerve;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Set;
@@ -90,8 +89,7 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Idx
   };
 
-  private Swerve swerve = new Swerve();
-  private DriveSubsystem driveSubsystem = new DriveSubsystem(swerve);
+  private DriveSubsystem driveSubsystem;
   /*Because we use two seperate loggers we can import one and then define the
   other here.*/
   private org.slf4j.Logger textLogger;
@@ -321,7 +319,10 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn
 
     // See what pose is closer the the gyro at the time of the photo's capture.
     double rotation =
-        gyroBuffer.get(FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime)));
+        gyroBuffer.get(
+            FastMath.floorToInt(
+                (((RobotController.getFPGATime() - time) / 1_000_000.0)
+                    / VisionConstants.kLoopTime)));
     return getCloserPose(pose1, pose2, rotation);
   }
 

From 7b7ac71a0766f6a376a9d011caef96995a34ad60 Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Mon, 24 Feb 2025 19:48:01 -0500
Subject: [PATCH 19/21] Fixed camera positions

---
 .../java/frc/robot/constants/VisionConstants.java    | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 81dd2d64..7b74af38 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -94,18 +94,18 @@ public final class VisionConstants {
       new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d());
   public static final Pose3d kCam2Pose =
       new Pose3d(
-          new Translation3d(0.236, -0.108, 0.932),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-45)));
+          new Translation3d(0.236, 0.108, 0.932),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(22.5)));
   public static final Pose3d kCam3Pose =
       new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d());
   public static final Pose3d kCam4Pose =
       new Pose3d(
-          new Translation3d(0.236, -0.038, 0.932),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
+          new Translation3d(0.236, -.038, 0.932),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(-22.5)));
   public static final Pose3d kCam5Pose =
       new Pose3d(
-          new Translation3d(-0.229, -0.073, 0.934),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
+          new Translation3d(-0.229, 0.073, 0.934),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(180)));
   // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
   // in the form [x, y, theta], with units in radians.
   public static Matrix<N1, N1> kLocalMeasurementStdDevs =

From 5341d4144f603bf3a8c65e0d2b6a8b8c3cae4e5b Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Mon, 24 Feb 2025 20:36:46 -0500
Subject: [PATCH 20/21] stuff

---
 .../java/frc/robot/constants/RobotConstants.java    | 13 +++++++++++++
 .../robot/subsystems/vision/VisionSubsystem.java    |  1 +
 2 files changed, 14 insertions(+)

diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java
index 84c97fff..04e4ca76 100644
--- a/src/main/java/frc/robot/constants/RobotConstants.java
+++ b/src/main/java/frc/robot/constants/RobotConstants.java
@@ -1,9 +1,22 @@
 package frc.robot.constants;
 
+import edu.wpi.first.wpilibj.RobotController;
+
 public class RobotConstants {
+  public static final String protoSerial = "032243F2";
+  public static final boolean isComp = !RobotController.getSerialNumber().equals(protoSerial);
+
   public static final int kTalonConfigTimeout = 10; // ms
 
   public static final double kJoystickDeadband = 0.1;
   public static final double kTriggerDeadband = 0.5;
   public static final double kTestingDeadband = 0.5;
+
+  public RobotConstants() {
+    if (isComp) {
+      // Fill with comp bot constants
+    } else {
+      // Fill with proto constants
+    }
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index f8942534..da2a7d69 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -328,6 +328,7 @@ private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIn
 
   @Override
   public void periodic() {
+    System.out.println(RobotController.getSerialNumber());
 
     double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians());
     gyroBuffer.addFirst(gyroData);

From 369fcf449f528bc4694d978f04dd56b9cf9123ec Mon Sep 17 00:00:00 2001
From: Huck-Richardson <huck.richardson@gmail.com>
Date: Thu, 27 Feb 2025 18:45:15 -0500
Subject: [PATCH 21/21] Robot Constants

---
 src/main/java/frc/robot/constants/RobotConstants.java | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java
index 04e4ca76..8837afa0 100644
--- a/src/main/java/frc/robot/constants/RobotConstants.java
+++ b/src/main/java/frc/robot/constants/RobotConstants.java
@@ -12,11 +12,18 @@ public class RobotConstants {
   public static final double kTriggerDeadband = 0.5;
   public static final double kTestingDeadband = 0.5;
 
+  public static double kZero = .36;
+
   public RobotConstants() {
     if (isComp) {
       // Fill with comp bot constants
     } else {
       // Fill with proto constants
+      kZero = ProtoConstants.kZero;
     }
   }
+
+  public static class ProtoConstants {
+    public static final double kZero = 36;
+  }
 }