diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 6540772..827c574 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/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 + } } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 5f27638..cf78866 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/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,6 +76,7 @@ public RobotContainer() { drivetrain::addVisionMeasurement, elevator::getPosition, drivetrain::getRotation); + noteVision = NoteVision.createReal(); } else { elevator = Elevator.createDummy(); drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0); @@ -84,6 +84,7 @@ public RobotContainer() { 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(); } } 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 97% 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..a591a6c 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; @@ -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 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/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/NoteVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVision.java new file mode 100644 index 0000000..84346d3 --- /dev/null +++ b/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() {}); + } +} 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..802b26f --- /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 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 ""; + } +} 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..25dfac7 --- /dev/null +++ b/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; + } +} 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/GamepieceDetection.java b/src/main/java/org/team1540/robot2024/util/vision/GamepieceDetection.java new file mode 100644 index 0000000..2a65288 --- /dev/null +++ b/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) {} 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;