Add note vision subsystem

src/main/java/org/team1540/robot2024/Constants.java

@@ -107,31 +107,33 @@ 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 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 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-note";
+            public static final int PIPELINE_INDEX = 0;
+            public static final Pose3d CAMERA_POSE = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); //TODO FIND THIS POSE
+        }
     }
 
 

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -28,12 +28,12 @@
 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.subsystems.vision.gamepiece.NoteVision;
 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;
 
@@ -48,6 +48,7 @@ public class RobotContainer {
     public final Elevator elevator;
     public final Indexer indexer;
     public final AprilTagVision aprilTagVision;
+    public final NoteVision noteVision;
     public final Leds leds = new Leds();
 
     // Controller
@@ -56,8 +57,6 @@ public class RobotContainer {
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
-
-
     public boolean isBrakeMode;
     /**
      * The container for the robot. Contains subsystems, IO devices, and commands.
@@ -77,13 +76,15 @@ public RobotContainer() {
                             drivetrain::addVisionMeasurement,
                             elevator::getPosition,
                             drivetrain::getRotation);
+                    noteVision = NoteVision.createReal();
                 } else {
                     elevator = Elevator.createDummy();
                     drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0);
                     tramp = Tramp.createDummy();
                     shooter = Shooter.createDummy();
                     indexer = Indexer.createDummy();
                     aprilTagVision = AprilTagVision.createDummy();
+                    noteVision = NoteVision.createDummy();
                 }
             }
             case SIM -> {
@@ -97,6 +98,7 @@ public RobotContainer() {
                         drivetrain::addVisionMeasurement,
                         drivetrain::getPose,
                         elevator::getPosition);
+                noteVision = NoteVision.createSim();
             }
             default -> {
                 // Replayed robot, disable IO implementations
@@ -106,6 +108,7 @@ public RobotContainer() {
                 shooter = Shooter.createDummy();
                 indexer = Indexer.createDummy();
                 aprilTagVision = AprilTagVision.createDummy();
+                noteVision = NoteVision.createDummy();
             }
         }
 

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java → src/main/java/org/team1540/robot2024/subsystems/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;
@@ -14,7 +14,7 @@
 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;
@@ -36,7 +36,7 @@ private AprilTagVision(
             AprilTagVisionIO rearCameraIO,
             Consumer<EstimatedVisionPose> visionPoseConsumer,
             Supplier<Double> elevatorHeightSupplierMeters) {
-        if (hasInstance) throw new IllegalStateException("Instance of vision already exists");
+        if (hasInstance) throw new IllegalStateException("Instance of AprilTagVision already exists");
         hasInstance = true;
 
         this.frontCameraIO = frontCameraIO;

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java → src/main/java/org/team1540/robot2024/subsystems/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;

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java → src/main/java/org/team1540/robot2024/subsystems/vision/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;

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java → src/main/java/org/team1540/robot2024/subsystems/vision/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;

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java → src/main/java/org/team1540/robot2024/subsystems/vision/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;

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java → src/main/java/org/team1540/robot2024/subsystems/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;

src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java

@@ -0,0 +1,48 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.team1540.robot2024.util.vision.GamepieceDetection;
+
+import static org.team1540.robot2024.Constants.Vision.Gamepiece.*;
+
+public class NoteVision extends SubsystemBase {
+    private final NoteVisionIO io;
+    private final NoteVisionIOInputsAutoLogged inputs = new NoteVisionIOInputsAutoLogged();
+
+    private static boolean hasInstance = false;
+
+    private NoteVision(NoteVisionIO io) {
+        if (hasInstance) throw new IllegalStateException("Instance of NoteVision already exists");
+        this.io = io;
+        hasInstance = true;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+    }
+
+    public GamepieceDetection getLatestDetection() {
+        return new GamepieceDetection(
+                inputs.lastDetectionTimestampSecs,
+                inputs.targetRotation.rotateBy(CAMERA_POSE.getRotation().unaryMinus()),
+                inputs.targetArea,
+                inputs.targetClass);
+    }
+
+    public boolean hasDetection() {
+        return inputs.hasDetection;
+    }
+
+    public static NoteVision createReal() {
+        return new NoteVision(new NoteVisionIOLimelight(CAMERA_NAME));
+    }
+
+    public static NoteVision createSim() {
+        return createDummy();
+    }
+
+    public static NoteVision createDummy() {
+        return new NoteVision(new NoteVisionIO() {});
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java

@@ -0,0 +1,21 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface NoteVisionIO {
+    @AutoLog
+    class NoteVisionIOInputs {
+        public boolean hasDetection = false;
+        public double lastDetectionTimestampSecs = 0.0;
+        public Rotation3d targetRotation = new Rotation3d(); // Camera-relative target rotation
+        public double targetArea = 0.0;
+        public String targetClass = "";
+    }
+
+    default void updateInputs(NoteVisionIOInputs inputs) {}
+
+    default String getName() {
+        return "";
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java

@@ -0,0 +1,41 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+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) {
+        inputs.hasDetection = LimelightHelpers.getTV(name);
+        if (inputs.hasDetection) {
+            inputs.lastDetectionTimestampSecs =
+                    (LimelightHelpers.getLimelightNTTableEntry(name, "tv").getLastChange() / 1000000.0)
+                            - ((LimelightHelpers.getLatency_Capture(name) + LimelightHelpers.getLatency_Pipeline(name))
+                            / 1000.0);
+            //TODO check rotation signs
+            inputs.targetRotation =
+                    new Rotation3d(
+                            0,
+                            -Math.toRadians(LimelightHelpers.getTY(name)),
+                            Math.toRadians(LimelightHelpers.getTX(name)));
+            inputs.targetArea = LimelightHelpers.getTA(name);
+            inputs.targetClass = LimelightHelpers.getNeuralClassID(name);
+        }
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+}

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);

src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java

@@ -0,0 +1,9 @@
+package org.team1540.robot2024.util.vision;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+
+public record GamepieceDetection(
+        double timestampSecs,
+        Rotation3d rotation, // Robot relative gamepiece rotation
+        double area,
+        String targetClass) {}

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;