.OutlineViewer/outlineviewer.json

@@ -1,40 +1,13 @@
 {
-  "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,7 +245,6 @@ 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);
 
@@ -321,17 +320,6 @@ 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,5 +8,4 @@ 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,12 +8,10 @@ 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{
@@ -27,9 +25,9 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
-    private DoubleSubscriber orange_tx;
+    private DoubleSubscriber orangeRobotRelativeX;
-    private DoubleSubscriber orange_ty;
+    private DoubleSubscriber orangeRobotRelativeY;
-    private DoubleSubscriber orange_dist;
+    private DoubleSubscriber orangeRobotRelativeZ;
 
     private DoubleSubscriber orangeClosestTag;
     private BooleanSubscriber orangeTagDetected;
@@ -54,9 +52,9 @@ public class Vision{
 
         blackFramerate = blackVisionTable.getDoubleTopic("blackFPS").subscribe(0.0);
 
-        orange_tx = orangeVisionTable.getDoubleTopic("tx").subscribe(0.0);
+        orangeRobotRelativeX = orangeVisionTable.getDoubleTopic("orangeRelativeX").subscribe(0.0);
-        orange_ty = orangeVisionTable.getDoubleTopic("ty").subscribe(0.0);
+        orangeRobotRelativeY = orangeVisionTable.getDoubleTopic("orangeRelativeY").subscribe(0.0);
-        orange_dist = orangeVisionTable.getDoubleTopic("totalDist").subscribe(0.0);
+        orangeRobotRelativeZ = orangeVisionTable.getDoubleTopic("orangeRelativeZ").subscribe(0.0);
 
         orangeClosestTag = orangeVisionTable.getDoubleTopic("orangeClosestTag").subscribe(0.0);
         orangeTagDetected = orangeVisionTable.getBooleanTopic("orangeTagDetected").subscribe(false);
@@ -80,9 +78,6 @@ 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));
@@ -104,8 +99,8 @@ public class Vision{
     }
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
-        return CameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
+        return relativeToGlobalPose2d(getBlackClosestTag(), 
-            getBlackRelativeX(), getBlackRelativeY(), Timer.getTimestamp(), gyroBuffer);
+            new Translation2d(getBlackRelativeX(), getBlackRelativeY()));
     }
 
     public double getBlackRelativeX(){
@@ -136,26 +131,21 @@ public class Vision{
         return blackFramerate.get();
     }
 
-    public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
+    public Pose2d getOrangeGlobalPose(){
-        if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
+        return relativeToGlobalPose2d(getOrangeClosestTag(), 
-            return CameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()));
-                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer
-                );
-        }else{
-            return new Pose2d();
-        }
     }
     
-    public double getOrangeTX(){
+    public double getOrangeRelativeX(){
-        return orange_tx.get();
+        return orangeRobotRelativeX.get();
     }
 
-    public double getOrangeTY(){
+    public double getOrangeRelativeY(){
-        return orange_ty.get();
+        return orangeRobotRelativeY.get();
     }
 
-    public double getOrangeDist(){
+    public double getOrangeRelativeZ(){
-        return orange_dist.get();
+        return orangeRobotRelativeZ.get();
     }
 
     public int getOrangeClosestTag(){
@@ -163,7 +153,7 @@ public class Vision{
     }
 
     public double getOrangeTimeStamp(){
-        return orange_tx.getLastChange();
+        return orangeRobotRelativeX.getLastChange();
     }
 
     public boolean getOrangeTagDetected(){