feat: add note vision subsystem

flamingchickens1540 · Sep 13, 2024 · 01012c0 · 01012c0
1 parent 9c0e9cc
commit 01012c0
Show file tree

Hide file tree

Showing 6 changed files with 68 additions and 6 deletions.
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -29,6 +29,7 @@
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 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;
@@ -47,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
@@ -55,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.
@@ -76,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 -> {
@@ -96,6 +98,7 @@ public RobotContainer() {
                         drivetrain::addVisionMeasurement,
                         drivetrain::getPose,
                         elevator::getPosition);
+                noteVision = NoteVision.createSim();
             }
             default -> {
                 // Replayed robot, disable IO implementations
@@ -105,6 +108,7 @@ public RobotContainer() {
                 shooter = Shooter.createDummy();
                 indexer = Indexer.createDummy();
                 aprilTagVision = AprilTagVision.createDummy();
+                noteVision = NoteVision.createDummy();
             }
         }
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java
@@ -9,7 +9,6 @@
 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;
@@ -37,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;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java
@@ -0,0 +1,50 @@
+package org.team1540.robot2024.subsystems.vision.gamepiece;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.team1540.robot2024.Constants;
+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.targetPitchRads,
+                inputs.targetYawRads,
+                inputs.targetAreaRads,
+                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() {});
+    }
+}
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
@@ -5,12 +5,12 @@
 public interface NoteVisionIO {
     @AutoLog
     class NoteVisionIOInputs {
+        public boolean hasDetection = false;
         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) {}

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
@@ -16,7 +16,8 @@ public NoteVisionIOLimelight(String name) {
 
     @Override
     public void updateInputs(NoteVisionIOInputs inputs) {
-        if (LimelightHelpers.getTV(name)) {
+        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))

diff --git a/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java b/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java
@@ -0,0 +1,8 @@
+package org.team1540.robot2024.util.vision;
+
+public record GamepieceDetection(
+        double timestampSecs,
+        double yawRads,
+        double pitchRads,
+        double areaRads,
+        String targetClass) {}