diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3094a08d..7b636186 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -531,6 +531,12 @@ public static class VisionConstants { public static final double ROBOT_TO_CAM2_YAW; public static final Transform3d ROBOT_TO_CAM2_3D; + //is camera connected check: + public static final int MIN_NUM_CAMERAS_DISCONNECTED = 2; + + //is vision updating check: + public static final double MAX_TIME_BETWEEN_UPDATES = 0.5; + // Camera 3 position - robot-specific public static final double ROBOT_TO_CAM3_X; public static final double ROBOT_TO_CAM3_Y; @@ -656,8 +662,8 @@ public static class VisionConstants { } public static class LEDConstants { // placeholder constants - public static final int KPORT = 0; - public static final int KLENGTH = 60; + public static final int KPORT = 9; + public static final int KLENGTH = 3*30; public static final double BLINK_TIME = 1; // in seconds for after intake/outtake } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6eaad219..152c4da7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.subsystems.scorer.ShooterSubsystem; import frc.robot.subsystems.LaserCANSensorBase; import frc.robot.subsystems.RobotCommandFactory; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.PivotConstants; import frc.robot.Constants.RollerConstants; @@ -101,6 +102,7 @@ public boolean isMagnitudeGreaterThan(double threshold) { public final IndexerSubsystem m_indexer = new IndexerSubsystem(); public final PivotSubsystem m_pivot = new PivotSubsystem(); public final ColumnSubsystem m_column = new ColumnSubsystem(); + public final LEDSubsystem m_led = new LEDSubsystem(); // Sensors private final LaserCANSensorBase m_intakeSensor = new LaserCANSensorBase( @@ -143,6 +145,12 @@ public RobotContainer() { configureBindingsCompetition(); configureBindingsVision(); } + + // Vision LED indicator + m_led.setDefaultCommand(m_led.runScrollingYellowBlueCommand()); + Trigger isVisionHealthy = new Trigger(() -> m_vision.isVisionHealthy()); + isVisionHealthy.whileTrue(m_led.runScrollingYellowBlueCommand()); + isVisionHealthy.whileFalse(m_led.runSolidRed()); // Configure auto builder createNamedCommands(); diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 2b4c3f3b..b29a928f 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -111,4 +111,5 @@ public Command runScrollingYellowBlueCommand() { LEDPattern pattern = base.scrollAtRelativeSpeed(Percent.per(Second).of(100)); return runPattern(pattern).withName("runScrollingYellowBlueCommand"); } + } diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index 949445ec..07620a2f 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -87,6 +87,10 @@ public Field2d getEstField(){ return m_estPoseField; } + public boolean isConnected() { + return m_camera.isConnected(); + } + // Toggles "Raw Video Mode" for single camera // NOTE: PhotonVision method for raw video mode is setDriverMode // setDriverMode(true) = raw video feed, setDriverMode(false) = normal AprilTag processing diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 97e76026..8f63fc1b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -64,6 +64,8 @@ public class VisionSubsystem extends SubsystemBase { private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing + private ArrayList m_lastEstPoses = new ArrayList<>(); + /** Creates a new Vision Subsystem. */ public VisionSubsystem(Supplier robotHeadingSupplier) { cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotHeadingSupplier)); @@ -110,6 +112,44 @@ public Command toggleRawVideoModeCommand() { }); } + public boolean areCamerasConnected() { + int notConnectedCounter = 0; + for (LocalizationCamera cam : cameras){ + if (!cam.isConnected()) + notConnectedCounter += 1; + if (notConnectedCounter > VisionConstants.MIN_NUM_CAMERAS_DISCONNECTED) + return false; + } + return true; + } + + public boolean isVisionUpdating() { + double currentTime = Timer.getFPGATimestamp(); + return (currentTime - m_lastTimestamp) < VisionConstants.MAX_TIME_BETWEEN_UPDATES; + } + + public boolean isEstPoseJumpy() { + if (m_lastEstPoses.size() < VisionConstants.NUM_LAST_EST_POSES) { + return true; + } + + double totalDistance = 0; + + for (int i = 0; i < m_lastEstPoses.size() - 1; i++) { + // add distance between ith pose and i+1th pose + totalDistance += Math.abs(m_lastEstPoses.get(i).minus(m_lastEstPoses.get(i + 1)).getTranslation().getNorm()); + } + + double avgDist = totalDistance / m_lastEstPoses.size(); + SmartDashboard.putNumber("vision/" + getName() + "/avgDistBetweenLastEstPoses", avgDist); + + return avgDist > VisionConstants.MAX_AVG_DIST_BETWEEN_LAST_EST_POSES; + } + + public boolean isVisionHealthy() { + return areCamerasConnected() && isVisionUpdating() && isEstPoseJumpy(); + } + @Override public void periodic() { // clear allValidReadings at the beginning of every loop