diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index 7da2f87..8b5ae11 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
@@ -32,8 +33,8 @@ public class Vision{
 
     private DoubleSubscriber orangeFramerate;
 
-    private double[] orangeCamPose = {0.0, -5.0, -10, 14.0-7.673, 1.05-14.0, 7.308+2.75};
-    private double[] blackCamPose = {0.0, -5.0, 10, 14.0-7.673, 1.05-14.0, 7.308+2.75};
+    private double[] orangeCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(-10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
+    private double[] blackCamPose = {0.0, Units.degreesToRadians(-5.0), Units.degreesToRadians(10), 14.0-7.673, 1.05-14.0, 7.308+2.75};
 
     public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
@@ -75,13 +76,13 @@ public class Vision{
         return new Pose2d(globalPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
     }
 
-    public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
+    public Pose2d cameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer, double[] camPose){
 
         System.out.println(gyroBuffer.getSample(timestamp));
 
         double distance2d = totalDist * Math.cos(-camPose[1] - ty);
 
-        Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0] - tx));
+        Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(-tx)));
 
         Pose2d tagPose2d = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
             VisionConstants.globalTagCoords[tagID][1],
@@ -90,17 +91,18 @@ public class Vision{
         Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi))
             .transformBy(new Transform2d(distance2d, 0.0, new Rotation2d()))
             .getTranslation();
-        Pose2d robotPose = new Pose2d(fieldToCameraTranslation,
-            new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[0])))
-            .transformBy(new Transform2d(new Pose2d(new Translation2d(), new Rotation2d(camPose[0])), Pose2d.kZero));
+        Pose2d robotPose = new Pose2d(
+            fieldToCameraTranslation,
+            new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(camPose[2])))
+            .transformBy(new Transform2d(new Pose2d(new Translation2d(camPose[3], camPose[4]), new Rotation2d(camPose[2])), Pose2d.kZero));
 
-        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d( gyroBuffer.getSample(timestamp).get()));
+        robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(gyroBuffer.getSample(timestamp).get()));
 
         return robotPose;
     }
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
-        return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
+        return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
             getBlackTX(), getBlackTY(), Timer.getTimestamp(), gyroBuffer, blackCamPose);
     }
 
@@ -134,7 +136,7 @@ public class Vision{
 
     public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
-            return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+            return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
                 orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
                 );
         }else{