I did a dumb with geometry

src/main/java/frc/robot/Constants.java

@@ -136,12 +136,13 @@ private DriveConstants() {
 
         public static final Transform3d RIGHT_CAM_POSE = new Transform3d(
                 new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(8.5), Units.inchesToMeters(6)),
-                new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(-18))
+                new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(18))
         );
 
         public static final Transform3d LEFT_CAM_POSE = new Transform3d(
                 new Translation3d(Units.inchesToMeters(10.5), Units.inchesToMeters(-8.5), Units.inchesToMeters(6)),
-                new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(18))
+                new Rotation3d(0.0, Units.degreesToRadians(50), Units.degreesToRadians(-18))
+//                new Rotation3d()
                 );
 
         // Tag Follow controllers

src/main/java/frc/robot/Robot.java

@@ -34,8 +34,6 @@ public class Robot extends LoggedRobot {
   private RobotContainer m_robotContainer;
   private PowerDistribution m_pdh;
 
-  private CameraPose cameraPose;
-
 
   private Timer m_timer;
   /**
@@ -93,7 +91,6 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-    cameraPose = new CameraPose("Test cam", new Translation3d(), new Rotation3d(1.5, 1.5, 1.5));
   }
 
   /**
@@ -110,9 +107,6 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
-    if (cameraPose.hasChanged()) {
-      cameraPose.updateVisualization();
-    }
 
     // Checks every 100 milliseconds (roughly 10 robot cycles) to see if any Spark Maxes have rebooted
     // if one has it will then rerun CAN ID configurations on it to stop CAN bus from overflowing

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import com.ctre.phoenix.led.*;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -13,6 +14,7 @@
 import frc.robot.commands.autonomous.AutoFactory;
 import frc.robot.commands.autonomous.Balance;
 import frc.robot.subsystems.arm.ArmExtSubsystem;
+import frc.robot.subsystems.vision.CameraSubsystem;
 import frc.robot.supersystems.ArmPose;
 import frc.robot.supersystems.ArmSupersystem;
 import lib.controllers.FootPedal;
@@ -47,6 +49,9 @@ public class RobotContainer {
     private FootPedal m_foot;
     private LedSubsystem m_led;
 
+    // Camera testing crap
+    private CameraSubsystem m_testingCamera;
+
     //Controllers
     private final CommandXboxController m_driveController = new CommandXboxController(Constants.DRIVER_PORT);
 //    private final CommandXboxController m_testController = new CommandXboxController(2);
@@ -71,6 +76,8 @@ public RobotContainer() {
                 m_ext = new ArmExtSubsystem();
                 m_super = new ArmSupersystem(m_arm, m_ext, m_wrist, m_drive);
                 m_foot = new FootPedal(1);
+
+                m_testingCamera = new CameraSubsystem("TestCamera", new Transform3d());
                 break;
 
             case SIM:

src/main/java/frc/robot/subsystems/vision/CameraPose.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.vision;
 
 import com.gos.lib.properties.GosDoubleProperty;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -14,6 +15,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
+import org.littletonrobotics.junction.Logger;
 
 import java.awt.*;
 
@@ -129,6 +131,8 @@ public CameraPose(String camName, Translation3d trans, Rotation3d rot) {
     }
 
     public boolean hasChanged() {
+        Logger.getInstance().recordOutput(m_camName + "/transform", new Pose3d(getTranslation(), getRotation()));
+
         if (m_lastx != m_x.getValue() ||
                 m_lasty != m_y.getValue() ||
                 m_lastz != m_z.getValue() ||

src/main/java/frc/robot/subsystems/vision/CameraSubsystem.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.vision;
 
 
+import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.*;
@@ -19,6 +20,7 @@
 
 import java.io.IOException;
 import java.util.ArrayList;
+import java.util.List;
 import java.util.Optional;
 
 
@@ -238,6 +240,20 @@ public int getTagID() {
         }
     }
 
+    public void useSingleTag(int tagId) {
+        AprilTagFieldLayout field = new AprilTagFieldLayout(
+                List.of(new AprilTag(tagId, new Pose3d())),
+                0.0,
+                0.0
+        );
+
+        m_photonPoseEstimator.setFieldTags(field);
+    }
+
+    public void resetTagField() {
+        m_photonPoseEstimator.setFieldTags(m_aprilTagFieldLayout);
+    }
+
     @Override
     public void periodic() {
         if (m_campose.hasChanged()) {