diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 18c5c84..bdb97c5 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -59,4 +59,6 @@ public class VisionConstants {
         {4.993, 2.816, 5.272, 2.996}
     };
 
+    public static final double latencyFudgeFactor = 0;
+
 }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 4fdca3f..436969b 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -65,6 +65,7 @@ public class Drivetrain extends SubsystemBase {
     public Vision vision;
 
     public Pose2d orangePose2d;
+    public Pose2d blackPose2d;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -148,6 +149,7 @@ public class Drivetrain extends SubsystemBase {
 
         vision = new Vision();
         orangePose2d = new Pose2d();
+        blackPose2d = new Pose2d();
     }
 
     @Override
@@ -175,24 +177,18 @@ public class Drivetrain extends SubsystemBase {
                 m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
             }
             Logger.recordOutput("orange pose", new Pose3d());
-            if(orangePose2d.equals(null)){
-                Logger.recordOutput("orange pose", new Pose3d());
-            }else{
-                Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
-            }
-
-            Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
         }
 
-        if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); 
+        if(vision.getBlackTagDetected()){
+            if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
 
-        }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
+            }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
+            }
+            Logger.recordOutput("black pose", new Pose3d(blackPose2d));
         }
         
         Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
@@ -309,7 +305,7 @@ public class Drivetrain extends SubsystemBase {
 
     public int getClosestTag(){
 
-		if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
+		if(DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue)){
 			int closestTag = 17;
 			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
 			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));