diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 6540772..74f5411 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 5f27638..cdd16ad 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 02ec382..cb87af6 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 0f3edff..0b969c6 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 064d67e..1526215 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 d3d84f4..93f48be 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 209de41..59bf161 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 cb27755..0abe468 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 0000000..1fa3a4d --- /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 0000000..718cb19 --- /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 628c79f..e7e8ae2 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 9d7d2f2..78fc371 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;