Feat: Added Update Inputs (Incomplete)

TitaniumTitans · Feb 3, 2024 · 68edbd1 · 68edbd1
1 parent eb5595c
commit 68edbd1
Show file tree

Hide file tree

Showing 2 changed files with 118 additions and 50 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLog;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -16,55 +17,122 @@
 import java.util.Optional;
 
 public class VisionSubsystem extends SubsystemBase {
-     private final PhotonCamera m_camera;
-     private final Transform3d robotToCam;
-     private PhotonPoseEstimator m_photonPoseEstimator;
-     private DriverStation.Alliance m_alliance;
-     private AprilTagFieldLayout m_aprilTagFieldLayout;
-
-     public class photonVisionInputs {
-        public boolean camConnected = false;
-        public double camLatency = 0.0;
-        public boolean tagsFound = false;
-        public int[] tagIds = new int[] {};
-        public int nearestTagId = 0;
-        public Pose2d nearestTagPose = new Pose2d();
+
+  @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 VisionSubsystem(String camName, Transform3d camPose) {
-        m_camera = new PhotonCamera(camName);
-        robotToCam = camPose;
-        if(DriverStation.getAlliance()!=null) {
-            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);
-            }
-            m_photonPoseEstimator = new PhotonPoseEstimator(
-                    m_aprilTagFieldLayout,
-                    PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
-                    m_camera,
-                    robotToCam);
-        } catch (IOException e){
-            throw new IllegalStateException(e);
-        }
+
+    public void setCamConnected(boolean camConnected) {
+      this.camConnected = camConnected;
+    }
+
+    public double getCamLatency() {
+      return camLatency;
+    }
+
+    public void setCamLatency(double camLatency) {
+      this.camLatency = camLatency;
+    }
+
+    public boolean isTagsFound() {
+      return tagsFound;
+    }
+
+    public void setTagsFound(boolean tagsFound) {
+      this.tagsFound = tagsFound;
+    }
+
+    public int[] getTagIds() {
+      return tagIds;
+    }
+
+    public void setTagIds(int[] tagIds) {
+      this.tagIds = tagIds;
     }
-    public Optional<EstimatedRobotPose> getPose(Pose2d prevEstimatedRobotPose){
-        m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
-        if(m_alliance != DriverStation.getAlliance().get());
+
+    public int getNumTagsFound() {
+      return numTagsFound;
+    }
+
+    public void setNumTagsFound(int numTagsFound) {
+      this.numTagsFound = numTagsFound;
+    }
+  }
+
+  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 void updateInputs(){
+    inputs.setCamConnected(m_camera.isConnected());
+    inputs.setCamLatency(m_camera.getLatestResult().getLatencyMillis());
+//    inputs.setTagIds(m_camera.getLatestResult().getTargets().);
+  }
+
+  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;
+    }
+    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);
+      }
+      m_photonPoseEstimator = new PhotonPoseEstimator(
+          m_aprilTagFieldLayout,
+          PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
+          m_camera,
+          robotToCam);
+    } catch (IOException e){
+      throw new IllegalStateException(e);
+    }
+  }
+
+  @Override
+  public void periodic() {
+    updateInputs();
+    super.periodic();
+  }
+
+  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.Blue){
-            m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
+
+        if (m_alliance == DriverStation.Alliance.Red) {
+          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
         } else {
-            m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
+          m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
         }
-        PhotonPipelineResult camResult = m_camera.getLatestResult();
-        return m_photonPoseEstimator.update(camResult);
+      }
     }
+    else {
+      m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide);
+    }
+
+    PhotonPipelineResult camResult = m_camera.getLatestResult();
+    return m_photonPoseEstimator.update(camResult);
+  }
 }
 
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
 {
   "fileName": "AdvantageKit.json",
   "name": "AdvantageKit",
-  "version": "3.0.0",
+  "version": "3.0.2",
   "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
   "frcYear": "2024",
   "mavenUrls": [],
@@ -10,24 +10,24 @@
     {
       "groupId": "org.littletonrobotics.akit.junction",
       "artifactId": "wpilib-shim",
-      "version": "3.0.0"
+      "version": "3.0.2"
     },
     {
       "groupId": "org.littletonrobotics.akit.junction",
       "artifactId": "junction-core",
-      "version": "3.0.0"
+      "version": "3.0.2"
     },
     {
       "groupId": "org.littletonrobotics.akit.conduit",
       "artifactId": "conduit-api",
-      "version": "3.0.0"
+      "version": "3.0.2"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "org.littletonrobotics.akit.conduit",
       "artifactId": "conduit-wpilibio",
-      "version": "3.0.0",
+      "version": "3.0.2",
       "skipInvalidPlatforms": false,
       "isJar": false,
       "validPlatforms": [
@@ -39,4 +39,4 @@
     }
   ],
   "cppDependencies": []
-}
+}