feat: notevision io classes

flamingchickens1540 · Sep 13, 2024 · 9c0e9cc · 9c0e9cc
1 parent 326ff0e
commit 9c0e9cc
Show file tree

Hide file tree

Showing 12 changed files with 103 additions and 42 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -107,31 +107,34 @@ public static class Indexer {
 
 
     public static class Vision {
-        public static final String FRONT_CAMERA_NAME = "limelight-front";
-        public static final String REAR_CAMERA_NAME = "limelight-rear";
-        public static final String VISION_CAMERA_NAME = "limelight-vision";
-        public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0));
-        public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(Math.PI, Math.toRadians(10), Math.PI+Math.toRadians(1.55)));
-        public static final Pose3d VISION_CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
-
-        public static final boolean TAKE_SNAPSHOTS = true;
-        public static final double SNAPSHOT_PERIOD_SECS = 1;
-
-        public static final double XY_STD_DEV_COEFF = 0.1;
-        public static final double ROT_STD_DEV_COEFF = 0.5;
-
-        public static final double MAX_AMBIGUITY_RATIO = 0.3;
-        public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
-        public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
-        public static final double MIN_ACCEPTED_NUM_TAGS = 1;
-        public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
-        public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;
-
-        public static final int SIM_RES_WIDTH = 1280;
-        public static final int SIM_RES_HEIGHT = 960;
-        public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(70);
-        public static final double SIM_FPS = 14.5;
-        public static final double SIM_AVG_LATENCY_MS = 100;
+        public static class AprilTag {
+            public static final String FRONT_CAMERA_NAME = "limelight-front";
+            public static final String REAR_CAMERA_NAME = "limelight-rear";
+            public static final String VISION_CAMERA_NAME = "limelight-vision";
+            public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0));
+            public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(Math.PI, Math.toRadians(10), Math.PI+Math.toRadians(1.55)));
+            public static final Pose3d VISION_CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
+            public static final boolean TAKE_SNAPSHOTS = true;
+            public static final double SNAPSHOT_PERIOD_SECS = 1;
+            public static final double XY_STD_DEV_COEFF = 0.1;
+            public static final double ROT_STD_DEV_COEFF = 0.5;
+            public static final double MAX_AMBIGUITY_RATIO = 0.3;
+            public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
+            public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
+            public static final double MIN_ACCEPTED_NUM_TAGS = 1;
+            public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
+            public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;
+            public static final int SIM_RES_WIDTH = 1280;
+            public static final int SIM_RES_HEIGHT = 960;
+            public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(70);
+            public static final double SIM_FPS = 14.5;
+            public static final double SIM_AVG_LATENCY_MS = 100;
+        }
+
+        public static class Gamepiece {
+            public static final String CAMERA_NAME = "limelight-notevision";
+            public static final int PIPELINE_INDEX = 0;
+        }
     }
 
 

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -28,12 +28,11 @@
 import org.team1540.robot2024.subsystems.led.patterns.*;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
-import org.team1540.robot2024.subsystems.vision.AprilTagVision;
+import org.team1540.robot2024.subsystems.vision.apriltag.AprilTagVision;
 import org.team1540.robot2024.util.CommandUtils;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.auto.AutoCommand;
 import org.team1540.robot2024.util.auto.AutoManager;
-import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 import java.util.function.BooleanSupplier;
 

diff --git a/...024/subsystems/vision/AprilTagVision.java → ...stems/vision/apriltag/AprilTagVision.java b/...024/subsystems/vision/AprilTagVision.java → ...stems/vision/apriltag/AprilTagVision.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -9,12 +9,13 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOInputsAutoLogged;
 import org.team1540.robot2024.util.vision.EstimatedVisionPose;
 
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-import static org.team1540.robot2024.Constants.Vision.*;
+import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
 
 public class AprilTagVision extends SubsystemBase {
     private final AprilTagVisionIO frontCameraIO;

diff --git a/...4/subsystems/vision/AprilTagVisionIO.java → ...ems/vision/apriltag/AprilTagVisionIO.java b/...4/subsystems/vision/AprilTagVisionIO.java → ...ems/vision/apriltag/AprilTagVisionIO.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import org.littletonrobotics.junction.AutoLog;

diff --git a/...ems/vision/AprilTagVisionIOLimelight.java → ...n/apriltag/AprilTagVisionIOLimelight.java b/...ems/vision/AprilTagVisionIOLimelight.java → ...n/apriltag/AprilTagVisionIOLimelight.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import org.team1540.robot2024.util.vision.LimelightHelpers;

diff --git a/...tems/vision/AprilTagVisionIOMegaTag2.java → ...on/apriltag/AprilTagVisionIOMegaTag2.java b/...tems/vision/AprilTagVisionIOMegaTag2.java → ...on/apriltag/AprilTagVisionIOMegaTag2.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;

diff --git a/...ystems/vision/AprilTagVisionIOPhoton.java → ...sion/apriltag/AprilTagVisionIOPhoton.java b/...ystems/vision/AprilTagVisionIOPhoton.java → ...sion/apriltag/AprilTagVisionIOPhoton.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -7,12 +7,13 @@
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
-import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.util.vision.AprilTagsCrescendo;
 
 import java.util.List;
 import java.util.Optional;
 
+import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
+
 public class AprilTagVisionIOPhoton implements AprilTagVisionIO {
     private final PhotonCamera camera;
     private final PhotonPoseEstimator photonEstimator;
@@ -42,7 +43,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
         for (PhotonTrackedTarget target : targets)
             if (target.getPoseAmbiguity() > maxAmbiguityRatio) maxAmbiguityRatio = target.getPoseAmbiguity();
 
-        if (estimatedPose.isPresent() && maxAmbiguityRatio < Constants.Vision.MAX_AMBIGUITY_RATIO) {
+        if (estimatedPose.isPresent() && maxAmbiguityRatio < MAX_AMBIGUITY_RATIO) {
             lastEstimatedPose = estimatedPose.get().estimatedPose;
             inputs.estimatedPoseMeters = lastEstimatedPose;
             inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds;

diff --git a/...ubsystems/vision/AprilTagVisionIOSim.java → .../vision/apriltag/AprilTagVisionIOSim.java b/...ubsystems/vision/AprilTagVisionIOSim.java → .../vision/apriltag/AprilTagVisionIOSim.java
@@ -1,4 +1,4 @@
-package org.team1540.robot2024.subsystems.vision;
+package org.team1540.robot2024.subsystems.vision.apriltag;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
@@ -13,14 +13,13 @@
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
-import org.team1540.robot2024.Constants;
 
 import java.io.IOException;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
 
-import static org.team1540.robot2024.Constants.Vision.*;
+import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
 
 public class AprilTagVisionIOSim implements AprilTagVisionIO {
     private final VisionSystemSim visionSystemSim;
@@ -77,7 +76,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
         for (PhotonTrackedTarget target : targets)
             if (target.getPoseAmbiguity() > maxAmbiguityRatio) maxAmbiguityRatio = target.getPoseAmbiguity();
 
-        if (estimatedPose.isPresent() && maxAmbiguityRatio < Constants.Vision.MAX_AMBIGUITY_RATIO) {
+        if (estimatedPose.isPresent() && maxAmbiguityRatio < MAX_AMBIGUITY_RATIO) {
             lastEstimatedPose = estimatedPose.get().estimatedPose;
             inputs.estimatedPoseMeters = lastEstimatedPose;
             inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java
@@ -0,0 +1,21 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface NoteVisionIO {
+    @AutoLog
+    class NoteVisionIOInputs {
+        public double lastDetectionTimestampSecs = 0.0;
+        public double targetPitchRads = 0.0;
+        public double targetYawRads = 0.0;
+        public double targetAreaRads = 0.0;
+        public String targetClass = "";
+        public double confidence = 0.0;
+    }
+
+    default void updateInputs(NoteVisionIOInputs inputs) {}
+
+    default String getName() {
+        return "";
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java
@@ -0,0 +1,35 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import org.team1540.robot2024.util.vision.LimelightHelpers;
+
+import static org.team1540.robot2024.Constants.Vision.Gamepiece.*;
+
+public class NoteVisionIOLimelight implements NoteVisionIO {
+    private final String name;
+
+    public NoteVisionIOLimelight(String name) {
+        this.name = name;
+        LimelightHelpers.setCameraMode_Processor(name);
+        LimelightHelpers.setLEDMode_PipelineControl(name);
+        LimelightHelpers.setPipelineIndex(name, PIPELINE_INDEX);
+    }
+
+    @Override
+    public void updateInputs(NoteVisionIOInputs inputs) {
+        if (LimelightHelpers.getTV(name)) {
+            inputs.lastDetectionTimestampSecs =
+                    (LimelightHelpers.getLimelightNTTableEntry(name, "tv").getLastChange() / 1000000.0)
+                            - ((LimelightHelpers.getLatency_Capture(name) + LimelightHelpers.getLatency_Pipeline(name))
+                            / 1000.0);
+            inputs.targetPitchRads = Math.toRadians(LimelightHelpers.getTY(name));
+            inputs.targetYawRads = Math.toRadians(LimelightHelpers.getTX(name));
+            inputs.targetAreaRads = LimelightHelpers.getTA(name);
+            inputs.targetClass = LimelightHelpers.getNeuralClassID(name);
+        }
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/EstimatedVisionPose.java
@@ -5,7 +5,8 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import org.team1540.robot2024.Constants;
+
+import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
 
 public class EstimatedVisionPose {
     public double timestampSecs = -1;
@@ -16,11 +17,11 @@ public class EstimatedVisionPose {
     public Matrix<N3, N1> getStdDevs() {
 
         double xyStdDev =
-                Constants.Vision.XY_STD_DEV_COEFF
+                XY_STD_DEV_COEFF
                     * Math.pow(avgDistance, 2.0)
                     / numTags;
         double rotStdDev =
-                Constants.Vision.ROT_STD_DEV_COEFF
+                ROT_STD_DEV_COEFF
                     * Math.pow(avgDistance, 2.0)
                     / numTags;
         return VecBuilder.fill(xyStdDev, xyStdDev, numTags > 1 ? rotStdDev : Double.POSITIVE_INFINITY);

diff --git a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java
@@ -5,6 +5,7 @@
 import java.util.function.Supplier;
 
 import static org.team1540.robot2024.Constants.Vision.*;
+import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
 
 public class VisionPoseAcceptor {
     private final Supplier<ChassisSpeeds> robotVelocitySupplier;