it gives values don't know if they are right

.OutlineViewer/outlineviewer.json

@@ -1,13 +1,40 @@
 {
+  "Clients": {
+    "open": true
+  },
   "NetworkTables Settings": {
     "mode": "Client (NT4)"
   },
+  "client@2": {
+    "open": true
+  },
+  "client@4": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@2": {
+    "Publishers": {
+      "open": true
+    },
+    "open": true
+  },
+  "outlineviewer@3": {
+    "open": true
+  },
+  "shuffleboard@1": {
+    "open": true
+  },
   "transitory": {
     "Shuffleboard": {
       "Sensors Tab": {
         "open": true
       },
       "open": true
+    },
+    "orange_Fiducial": {
+      "open": true
     }
   }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -245,6 +245,7 @@ public class RobotContainer {
     private void configureShuffleboard() {
 		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
+		ShuffleboardTab apriltagTab = Shuffleboard.getTab(OIConstants.kApriltagTab);
 
 		Shuffleboard.selectTab(OIConstants.kAutoTab);
 
@@ -320,6 +321,17 @@ public class RobotContainer {
 
 		//sensorTab.add("odometry", drivetrain::getPose);
 
+		apriltagTab.addDouble("Orange ID", vision::getOrangeClosestTag);
+		apriltagTab.addDouble("Orange tx", vision::getOrangeTX);
+		apriltagTab.addDouble("Orange ty", vision::getOrangeTY);
+		apriltagTab.addDouble("Orange dist", vision::getOrangeDist);
+		apriltagTab.addDouble("global x", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX());
+		apriltagTab.addDouble("global y", () -> vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY());
+
+
+//		sensorTab.addDouble(" ID", vision::getOrangeClosestTag);
+
+
  	}
 
 	public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/OIConstants.java

@@ -8,4 +8,5 @@ public class OIConstants {
 
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
+    public static final String kApriltagTab = "Apriltag Tab";
 }

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

@@ -8,10 +8,12 @@ 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;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.VisionConstants;
 
 public class Vision{
@@ -25,9 +27,9 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orangeRobotRelativeX;
-    private DoubleSubscriber orangeRobotRelativeY;
-    private DoubleSubscriber orangeRobotRelativeZ;
+    private DoubleSubscriber orange_tx;
+    private DoubleSubscriber orange_ty;
+    private DoubleSubscriber orange_dist;
 
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
@@ -52,9 +54,9 @@ public class Vision{
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
-        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
-        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
+        orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
 
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
@@ -78,6 +80,9 @@ public class Vision{
     }
 
     public Pose2d CameraToGlobalPose2d(int tagID, double totalDist, double tx, double ty, double timestamp, TimeInterpolatableBuffer<Double> gyroBuffer){
+
+        System.out.println(gyroBuffer.getSample(timestamp));
+
         double distance2d = totalDist * Math.cos(-orangeCamPose[1] - ty);
 
         Rotation2d camToTagRotation = new Rotation2d(gyroBuffer.getSample(timestamp).get()).plus(new Rotation2d(orangeCamPose[0] - tx));
@@ -99,8 +104,8 @@ public class Vision{
     }
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
-        return relativeToGlobalPose2d(getBlackClosestTag(), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
+        return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
+            getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer);
     }
 
     public double getBlackRelativeX(){
@@ -131,21 +136,26 @@ public class Vision{
         return blackFramerate.get();
     }
 
-    public Pose2d getOrangeGlobalPose(){
-        return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
+    public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+        if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
+            return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer
+                );
+        }else{
+            return new Pose2d();
+        }
     }
     
-    public double getOrangeRelativeX(){
-        return orangeRobotRelativeX.get();
+    public double getOrangeTX(){
+        return orange_tx.get();
     }
 
-    public double getOrangeRelativeY(){
-        return orangeRobotRelativeY.get();
+    public double getOrangeTY(){
+        return orange_ty.get();
     }
 
-    public double getOrangeRelativeZ(){
-        return orangeRobotRelativeZ.get();
+    public double getOrangeDist(){
+        return orange_dist.get();
     }
 
     public int getOrangeClosestTag(){
@@ -153,7 +163,7 @@ public class Vision{
     }
 
     public double getOrangeTimeStamp(){
-        return orangeRobotRelativeX.getLastChange();
+        return orange_tx.getLastChange();
     }
 
     public boolean getOrangeTagDetected(){