added photon camera disconnected alerts

Shenzhen-Robotics-Alliance · Aug 7, 2024 · a23175c · a23175c
1 parent 85149ce
commit a23175c
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drive/SwerveDrive.java
@@ -43,7 +43,7 @@ public class SwerveDrive extends MapleSubsystem implements HolonomicDriveSubsyst
     private final SwerveDrivePoseEstimator poseEstimator;
 
     private final OdometryThread odometryThread;
-    private final Alert gyroDisconnected = new Alert("Gyro Hardware Fault", Alert.AlertType.ERROR);
+    private final Alert gyroDisconnectedAlert = new Alert("Gyro Hardware Fault", Alert.AlertType.ERROR);
     public SwerveDrive(GyroIO gyroIO, ModuleIO frontLeftModuleIO, ModuleIO frontRightModuleIO, ModuleIO backLeftModuleIO, ModuleIO backRightModuleIO, MapleConfigFile.ConfigBlock generalConfigBlock) {
         super("Drive");
         this.gyroIO = gyroIO;
@@ -82,7 +82,7 @@ kinematics, rawGyroRotation, lastModulePositions, new Pose2d(),
         this.odometryThreadInputs = new OdometryThreadInputsAutoLogged();
         this.odometryThread.start();
 
-        gyroDisconnected.setActivated(false);
+        gyroDisconnectedAlert.setActivated(false);
     }
 
     @Override
@@ -111,7 +111,7 @@ private void fetchOdometryInputs() {
 
         gyroIO.updateInputs(gyroInputs);
         Logger.processInputs("Drive/Gyro", gyroInputs);
-        gyroDisconnected.setActivated(!gyroInputs.connected);
+        gyroDisconnectedAlert.setActivated(!gyroInputs.connected);
 
         odometryThread.unlockOdometry();
     }

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java
@@ -30,17 +30,17 @@ public class SwerveModule extends MapleSubsystem {
     private SwerveModuleState setPoint;
     private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{};
 
-    private final Alert alert;
+    private final Alert hardwareFaultAlert;
 
     public SwerveModule(ModuleIO io, String name) {
         super("Module-" + name);
         this.io = io;
         this.name = name;
-        this.alert = new Alert(
+        this.hardwareFaultAlert = new Alert(
                 "Module-" + name + " Hardware Fault",
                 Alert.AlertType.ERROR
         );
-        this.alert.setActivated(false);
+        this.hardwareFaultAlert.setActivated(false);
 
         driveOpenLoop = InterpolatedMotorFeedForward.fromDeployedDirectory("DrivingMotorOpenLoop");
         turnCloseLoop = new MaplePIDController(Constants.SwerveModuleConfigs.steerHeadingCloseLoopConfig);
@@ -61,7 +61,7 @@ public void onReset() {
     public void updateOdometryInputs() {
         io.updateInputs(inputs);
         Logger.processInputs("Drive/Module-" + name, inputs);
-        this.alert.setActivated(!inputs.hardwareConnected);
+        this.hardwareFaultAlert.setActivated(!inputs.hardwareConnected);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java
@@ -1,9 +1,9 @@
 package frc.robot.subsystems.vision.apriltags;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
+import frc.robot.utils.Alert;
 import frc.robot.utils.Config.PhotonCameraProperties;
 import org.littletonrobotics.junction.Logger;
 
@@ -21,10 +21,19 @@ public class AprilTagVision extends MapleSubsystem {
 
     private final MapleMultiTagPoseEstimator multiTagPoseEstimator;
     private final HolonomicDriveSubsystem driveSubsystem;
+    private final Alert[] camerasDisconnectedAlerts;
     public AprilTagVision(AprilTagVisionIO io, List<PhotonCameraProperties> camerasProperties, HolonomicDriveSubsystem driveSubsystem) {
         super("Vision");
         this.io = io;
         this.inputs = new AprilTagVisionIO.VisionInputs(camerasProperties.size());
+        this.camerasDisconnectedAlerts = new Alert[camerasProperties.size()];
+        for (int i = 0; i < camerasProperties.size(); i++) {
+            this.camerasDisconnectedAlerts[i] = new Alert(
+                    "Photon Camera " + i + " '" + camerasProperties.get(i).name + "' disconnected",
+                    Alert.AlertType.WARNING
+            );
+            this.camerasDisconnectedAlerts[i].setActivated(false);
+        }
 
         this.multiTagPoseEstimator = new MapleMultiTagPoseEstimator(
                 fieldLayout,
@@ -44,6 +53,9 @@ public void periodic(double dt, boolean enabled) {
         io.updateInputs(inputs);
         Logger.processInputs(APRIL_TAGS_VISION_PATH + "Inputs", inputs);
 
+        for (int i = 0; i < inputs.camerasInputs.length; i++)
+            this.camerasDisconnectedAlerts[i].setActivated(!inputs.camerasInputs[i].cameraConnected);
+
         Optional<RobotPoseEstimationResult> result = multiTagPoseEstimator.estimateRobotPose(inputs.camerasInputs, driveSubsystem.getPose());
         result = discardResultIfOverThreshold(result);
         result.ifPresent(robotPoseEstimationResult -> driveSubsystem.addVisionMeasurement(

diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIO.java b/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIO.java
@@ -6,10 +6,7 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
-import java.util.Arrays;
-
 public interface AprilTagVisionIO {
-    int MAX_SUPPORTED_CAMERA_AMOUNT = 10;
     class CameraInputs {
         public boolean cameraConnected;
         public double resultsDelaySeconds;
@@ -67,6 +64,7 @@ class VisionInputs implements LoggableInputs {
         public final int camerasAmount;
         public final CameraInputs[] camerasInputs;
         public double inputsFetchedRealTimeStampSeconds = 0;
+        public boolean coprocessorConnected = false;
 
         public VisionInputs(int camerasAmount) {
             this.camerasAmount = camerasAmount;

diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIOReal.java
@@ -10,8 +10,8 @@
 public class AprilTagVisionIOReal implements AprilTagVisionIO {
     protected final PhotonCamera[] cameras;
     public AprilTagVisionIOReal(List<PhotonCameraProperties> cameraProperties) {
-        if (cameraProperties.size() > MAX_SUPPORTED_CAMERA_AMOUNT)
-            throw new IllegalArgumentException("max supported camera count is 10");
+        if (cameraProperties.size() > 16)
+            throw new IllegalArgumentException("max supported camera count is 16");
         cameras = new PhotonCamera[cameraProperties.size()];
 
         for (int i = 0; i < cameraProperties.size(); i++)