commented out archaic bug fix

TitaniumTitans · Feb 8, 2024 · e23128d · e23128d
1 parent 5c38444
commit e23128d
Show file tree

Hide file tree

Showing 2 changed files with 78 additions and 74 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java
@@ -0,0 +1,52 @@
+package frc.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+@AutoLog
+public class PhotonVisionIOInputs {
+  protected boolean camConnected = false;
+  protected double camLatency = 0.0;
+  protected boolean tagsFound = false;
+  protected int[] tagIds = new int[] {};
+  protected int numTagsFound = 0;
+
+  public boolean isCamConnected() {
+    return camConnected;
+  }
+
+  public void setCamConnected(boolean camConnected) {
+    this.camConnected = camConnected;
+  }
+
+  public double getCamLatency() {
+    return camLatency;
+  }
+
+  public void setCamLatency(double camLatency) {
+    this.camLatency = camLatency;
+  }
+
+  public int[] getTagIds() {
+    return tagIds;
+  }
+
+  public void setTagIds(int[] tagIds) {
+    this.tagIds = tagIds;
+  }
+
+  public int getNumTagsFound() {
+    return numTagsFound;
+  }
+
+  public void setNumTagsFound(int numTagsFound) {
+    this.numTagsFound = numTagsFound;
+  }
+
+  public boolean isTagsFound() {
+    return tagsFound;
+  }
+
+  public void setTagsFound(boolean tagsFound) {
+    this.tagsFound = tagsFound;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -18,62 +18,13 @@
 
 public class VisionSubsystem extends SubsystemBase {
 
-  @AutoLog
-  public class PhotonVisionIO {
-    protected boolean camConnected = false;
-    protected double camLatency = 0.0;
-    protected boolean tagsFound = false;
-    protected int[] tagIds = new int[] {};
-    protected int numTagsFound = 0;
-
-    public boolean isCamConnected() {
-      return camConnected;
-    }
-
-    public void setCamConnected(boolean camConnected) {
-      this.camConnected = camConnected;
-    }
-
-    public double getCamLatency() {
-      return camLatency;
-    }
-
-    public void setCamLatency(double camLatency) {
-      this.camLatency = camLatency;
-    }
-
-    public int[] getTagIds() {
-      return tagIds;
-    }
-
-    public void setTagIds(int[] tagIds) {
-      this.tagIds = tagIds;
-    }
-
-    public int getNumTagsFound() {
-      return numTagsFound;
-    }
-
-    public void setNumTagsFound(int numTagsFound) {
-      this.numTagsFound = numTagsFound;
-    }
-
-    public boolean isTagsFound() {
-      return tagsFound;
-    }
-
-    public void setTagsFound(boolean tagsFound) {
-      this.tagsFound = tagsFound;
-    }
-  }
-
   private final PhotonCamera m_camera;
   private final Transform3d robotToCam;
   private PhotonPoseEstimator m_photonPoseEstimator;
   private DriverStation.Alliance m_alliance;
   private AprilTagFieldLayout m_aprilTagFieldLayout;
 
-  private final PhotonVisionIO inputs = new PhotonVisionIO();
+  private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged();
 
   public void updateInputs(){
     inputs.setCamConnected(m_camera.isConnected());
@@ -86,18 +37,18 @@ public void updateInputs(){
   public VisionSubsystem(String camName, Transform3d camPose) {
     m_camera = new PhotonCamera(camName);
     robotToCam = camPose;
-    if(DriverStation.getAlliance().isPresent()) {
-      m_alliance = DriverStation.getAlliance().get();
-    } else {
-      m_alliance = DriverStation.Alliance.Blue;
-    }
+//    if(DriverStation.getAlliance().isPresent()) {
+//      m_alliance = DriverStation.getAlliance().get();
+//    } else {
+//      m_alliance = DriverStation.Alliance.Blue;
+//    }
     try {
       m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
-      if(m_alliance == DriverStation.Alliance.Blue){
-        m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
-      } else {
-        m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
-      }
+//      if(m_alliance == DriverStation.Alliance.Blue){
+//        m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
+//      } else {
+//        m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
+//      }
       m_photonPoseEstimator = new PhotonPoseEstimator(
           m_aprilTagFieldLayout,
           PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
@@ -118,20 +69,21 @@ public Optional<EstimatedRobotPose> getPose(Pose2d prevEstimatedRobotPose) {
 
     m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
 
-    if(DriverStation.getAlliance().isPresent()){
-      if (m_alliance != DriverStation.getAlliance().get()){
-        m_alliance = DriverStation.getAlliance().get();
-
-        if (m_alliance == DriverStation.Alliance.Red) {
-          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
-        } else {
-          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
-        }
-      }
-    }
-    else {
-      m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
-    }
+    // We shouldn't have to do this anymore, as pathplanners new lib doesn't have the weird origin thing
+//    if(DriverStation.getAlliance().isPresent()){
+//      if (m_alliance != DriverStation.getAlliance().get()){
+//        m_alliance = DriverStation.getAlliance().get();
+//
+//        if (m_alliance == DriverStation.Alliance.Red) {
+//          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
+//        } else {
+//          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
+//        }
+//      }
+//    }
+//    else {
+//      m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
+//    }
 
     PhotonPipelineResult camResult = m_camera.getLatestResult();
     return m_photonPoseEstimator.update(camResult);