global pose vision transformations

src/main/java/frc/robot/subsystems/Vision.java

@@ -1,13 +1,23 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+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.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
-    private DoubleSubscriber blackRobotRelativePose;
+    private DoubleSubscriber blackRobotRelativeX;
+    private DoubleSubscriber blackRobotRelativeY;
+    private DoubleSubscriber blackRobotRelativeZ;
+
     private DoubleSubscriber blackClosestTag;
     private BooleanSubscriber blackTagDetected;
 
@@ -22,13 +32,18 @@ public class Vision{
 
     private DoubleSubscriber orangeFramerate;
 
-    public Vision(){
+    private DoubleSupplier gyroAngle;
+
+    public Vision(DoubleSupplier gyroAngle){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
         NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
 
-        blackRobotRelativePose = blackVisionTable.getDoubleTopic("blackRelativePose").subscribe(0.0);
+        blackRobotRelativeX = orangeVisionTable.getDoubleTopic("blackRelativeX").subscribe(0.0);
+        blackRobotRelativeY = orangeVisionTable.getDoubleTopic("blackRelativeY").subscribe(0.0);
+        blackRobotRelativeZ = orangeVisionTable.getDoubleTopic("blackRelativeZ").subscribe(0.0);
+
         blackClosestTag = blackVisionTable.getDoubleTopic("blackClosestTag").subscribe(0.0);
         blackTagDetected = blackVisionTable.getBooleanTopic("blackTagDetected").subscribe(false);
 
@@ -43,17 +58,46 @@ public class Vision{
 
         orangeFramerate = orangeVisionTable.getDoubleTopic("orangeFPS").subscribe(0.0);
     }
+    
+    public Pose2d relativeToGlobalPose2d(int tagID, Translation2d relativeCoords, Rotation2d gyroAngle){
+        Pose2d tag2dPose = new Pose2d(VisionConstants.globalTagCoords[tagID][0],
+            VisionConstants.globalTagCoords[tagID][1],
+            new Rotation2d());
 
-    public double getBlackGlobalPose(){
-        return blackRobotRelativePose.get();
+        Pose2d relative = new Pose2d(relativeCoords, gyroAngle);
+
+        Transform2d relative2dTransformation = new Transform2d(relative.getTranslation(), relative.getRotation());
+
+        Pose2d globalPose = tag2dPose.transformBy(relative2dTransformation.inverse());
+
+        return new Pose2d(globalPose.getTranslation(), gyroAngle);
     }
 
-    public double getBlackClosestTag(){
-        return blackClosestTag.get();
+    public Pose2d getBlackGlobalPose(){
+        
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
+    }
+
+    public double getBlackRelativeX(){
+        return blackRobotRelativeX.get();
+    }
+
+    public double getBlackRelativeY(){
+        return blackRobotRelativeY.get();
+    }
+
+    public double getBlackRelativeZ(){
+        return blackRobotRelativeZ.get();
+    }
+
+    public int getBlackClosestTag(){
+        return (int) blackClosestTag.get();
     }
 
     public double getBlackTimeStamp(){
-        return blackRobotRelativePose.getLastChange();
+        return blackRobotRelativeX.getLastChange();
     }
 
     public boolean getBlackTagDetected(){
@@ -63,13 +107,28 @@ public class Vision{
     public double getBlackFPS(){
         return blackFramerate.get();
     }
+
+    public Pose2d getOrangeGlobalPose(){
+        
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
+            new Rotation2d(gyroAngle.getAsDouble()));
+    }
     
-    public double getOrangeGlobalX(){
+    public double getOrangeRelativeX(){
         return orangeRobotRelativeX.get();
     }
 
-    public double getOrangeClosestTag(){
-        return orangeClosestTag.get();
+    public double getOrangeRelativeY(){
+        return orangeRobotRelativeY.get();
+    }
+
+    public double getOrangeRelativeZ(){
+        return orangeRobotRelativeZ.get();
+    }
+
+    public int getOrangeClosestTag(){
+        return (int) orangeClosestTag.get();
     }
 
     public double getOrangeTimeStamp(){