From 9c0e9cc4489990787b89d2dca39dad4342477b8d Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Fri, 13 Sep 2024 10:20:32 -0700 Subject: [PATCH 1/5] feat: notevision io classes --- .../org/team1540/robot2024/Constants.java | 53 ++++++++++--------- .../team1540/robot2024/RobotContainer.java | 3 +- .../vision/{ => apriltag}/AprilTagVision.java | 5 +- .../{ => apriltag}/AprilTagVisionIO.java | 2 +- .../AprilTagVisionIOLimelight.java | 2 +- .../AprilTagVisionIOMegaTag2.java | 2 +- .../AprilTagVisionIOPhoton.java | 7 +-- .../{ => apriltag}/AprilTagVisionIOSim.java | 7 ++- .../vision/gamepiece/NoteVisionIO.java | 21 ++++++++ .../gamepiece/NoteVisionIOLimelight.java | 35 ++++++++++++ .../util/vision/EstimatedVisionPose.java | 7 +-- .../util/vision/VisionPoseAcceptor.java | 1 + 12 files changed, 103 insertions(+), 42 deletions(-) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVision.java (96%) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVisionIO.java (92%) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVisionIOLimelight.java (97%) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVisionIOMegaTag2.java (97%) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVisionIOPhoton.java (93%) rename src/main/java/org/team1540/robot2024/subsystems/vision/{ => apriltag}/AprilTagVisionIOSim.java (94%) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIO.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 65407720..74f54119 100644 --- 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 index 5f276386..cdd16adf 100644 --- 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/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java similarity index 96% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java index 02ec3829..cb87af6c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java +++ b/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; @@ -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/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIO.java similarity index 92% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIO.java index 0f3edff7..0b969c66 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java +++ b/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; diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOLimelight.java similarity index 97% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOLimelight.java index 064d67e5..15262159 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java +++ b/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; diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java similarity index 97% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java index d3d84f44..93f48beb 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java +++ b/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; diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOPhoton.java similarity index 93% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOPhoton.java index 209de410..59bf1617 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java +++ b/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; diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOSim.java similarity index 94% rename from src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java rename to src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOSim.java index cb277555..0abe4682 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java +++ b/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; 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 new file mode 100644 index 00000000..1fa3a4d3 --- /dev/null +++ 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 new file mode 100644 index 00000000..718cb190 --- /dev/null +++ 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 index 628c79f2..e7e8ae21 100644 --- 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 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 index 9d7d2f25..78fc3715 100644 --- 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 robotVelocitySupplier; 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 2/5] 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 cdd16adf..cf78866a 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 cb87af6c..a591a6c0 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 00000000..17529533 --- /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 1fa3a4d3..5ca7abe1 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 718cb190..eb629668 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 00000000..2b22aa34 --- /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) {} From c92b95702d65c593c413fb8c9d1ddf9f8d950d61 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Sat, 14 Sep 2024 18:01:39 -0700 Subject: [PATCH 3/5] fix: correct target area units --- .../robot2024/subsystems/vision/gamepiece/NoteVision.java | 3 +-- .../robot2024/subsystems/vision/gamepiece/NoteVisionIO.java | 2 +- .../subsystems/vision/gamepiece/NoteVisionIOLimelight.java | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) 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 index 17529533..d8335d70 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java @@ -1,7 +1,6 @@ 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.*; @@ -28,7 +27,7 @@ public GamepieceDetection getLatestDetection() { inputs.lastDetectionTimestampSecs, inputs.targetPitchRads, inputs.targetYawRads, - inputs.targetAreaRads, + inputs.targetArea, inputs.targetClass); } 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 5ca7abe1..f5d81492 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 @@ -9,7 +9,7 @@ class NoteVisionIOInputs { public double lastDetectionTimestampSecs = 0.0; public double targetPitchRads = 0.0; public double targetYawRads = 0.0; - public double targetAreaRads = 0.0; + public double targetArea = 0.0; public String targetClass = ""; } 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 eb629668..04eac6ea 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 @@ -24,7 +24,7 @@ public void updateInputs(NoteVisionIOInputs inputs) { / 1000.0); inputs.targetPitchRads = Math.toRadians(LimelightHelpers.getTY(name)); inputs.targetYawRads = Math.toRadians(LimelightHelpers.getTX(name)); - inputs.targetAreaRads = LimelightHelpers.getTA(name); + inputs.targetArea = LimelightHelpers.getTA(name); inputs.targetClass = LimelightHelpers.getNeuralClassID(name); } } From eb4eb2177bc1fbf312640887050624ab4a7ab2ce Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 16 Sep 2024 09:59:32 -0700 Subject: [PATCH 4/5] fix: use rotation3ds for returning gamepiece positions --- src/main/java/org/team1540/robot2024/Constants.java | 3 +-- .../subsystems/vision/gamepiece/NoteVision.java | 3 +-- .../subsystems/vision/gamepiece/NoteVisionIO.java | 4 ++-- .../vision/gamepiece/NoteVisionIOLimelight.java | 9 +++++++-- .../robot2024/util/vision/GamepieceDetection.java | 7 ++++--- 5 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 74f54119..e7c60737 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -110,10 +110,8 @@ public static class Vision { 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; @@ -134,6 +132,7 @@ public static class AprilTag { public static class Gamepiece { public static final String CAMERA_NAME = "limelight-notevision"; 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 } } 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 index d8335d70..84346d3c 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java @@ -25,8 +25,7 @@ public void periodic() { public GamepieceDetection getLatestDetection() { return new GamepieceDetection( inputs.lastDetectionTimestampSecs, - inputs.targetPitchRads, - inputs.targetYawRads, + inputs.targetRotation.rotateBy(CAMERA_POSE.getRotation().unaryMinus()), inputs.targetArea, inputs.targetClass); } 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 f5d81492..802b26f2 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 @@ -1,5 +1,6 @@ package org.team1540.robot2024.subsystems.vision.gamepiece; +import edu.wpi.first.math.geometry.Rotation3d; import org.littletonrobotics.junction.AutoLog; public interface NoteVisionIO { @@ -7,8 +8,7 @@ public interface NoteVisionIO { class NoteVisionIOInputs { public boolean hasDetection = false; public double lastDetectionTimestampSecs = 0.0; - public double targetPitchRads = 0.0; - public double targetYawRads = 0.0; + public Rotation3d targetRotation = new Rotation3d(); // Camera-relative target rotation public double targetArea = 0.0; public String targetClass = ""; } 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 04eac6ea..25dfac7b 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 @@ -1,5 +1,6 @@ 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.*; @@ -22,8 +23,12 @@ public void updateInputs(NoteVisionIOInputs inputs) { (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)); + //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); } diff --git a/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java b/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java index 2b22aa34..2a652882 100644 --- a/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java +++ b/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java @@ -1,8 +1,9 @@ package org.team1540.robot2024.util.vision; +import edu.wpi.first.math.geometry.Rotation3d; + public record GamepieceDetection( double timestampSecs, - double yawRads, - double pitchRads, - double areaRads, + Rotation3d rotation, // Robot relative gamepiece rotation + double area, String targetClass) {} From 9648739885fa006bc6606293ed75f8588d796ad9 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Mon, 16 Sep 2024 15:57:42 -0700 Subject: [PATCH 5/5] fix: make limelight name shorter --- src/main/java/org/team1540/robot2024/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index e7c60737..827c5744 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -130,7 +130,7 @@ public static class AprilTag { } public static class Gamepiece { - public static final String CAMERA_NAME = "limelight-notevision"; + 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 }