From 01012c077c1ff72f9c93a26cd8d26bf856af1d3f Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 13 Sep 2024 14:08:09 -0700 Subject: [PATCH] feat: add note vision subsystem --- .../team1540/robot2024/RobotContainer.java | 8 ++- .../vision/apriltag/AprilTagVision.java | 3 +- .../vision/gamepiece/NoteVision.java | 50 +++++++++++++++++++ .../vision/gamepiece/NoteVisionIO.java | 2 +- .../gamepiece/NoteVisionIOLimelight.java | 3 +- .../util/vision/GamepieceDetection.java | 8 +++ 6 files changed, 68 insertions(+), 6 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java create mode 100644 src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index cdd16ad..cf78866 100644 --- 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,6 +76,7 @@ public RobotContainer() { drivetrain::addVisionMeasurement, elevator::getPosition, drivetrain::getRotation); + noteVision = NoteVision.createReal(); } else { elevator = Elevator.createDummy(); drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0); @@ -83,6 +84,7 @@ public RobotContainer() { 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 index cb87af6..a591a6c 100644 --- 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 visionPoseConsumer, Supplier 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 new file mode 100644 index 0000000..1752953 --- /dev/null +++ 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 index 1fa3a4d..5ca7abe 100644 --- 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 index 718cb19..eb62966 100644 --- 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 new file mode 100644 index 0000000..2b22aa3 --- /dev/null +++ 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) {}