leds for vision indication

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
   }

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();

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");
   }
+
 }

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

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<Pose2d> m_lastEstPoses = new ArrayList<>();
+
   /** Creates a new Vision Subsystem. */
   public VisionSubsystem(Supplier<Rotation2d> 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