Make everything use our Subsystem class, improve LED controller

src/main/java/frc4388/robot/Constants.java

@@ -357,7 +357,6 @@ public final class Constants {
         public static final int L4_DRIVE_TIME = 250; //Milliseconds
         // public static final int L3_DRIVE_TIME = 500;
         public static final int L2_DRIVE_TIME = 250; //Milliseconds
-
         public static final int ALGAE_DRIVE_TIME = 500;
     }
 
@@ -372,7 +371,8 @@ public final class Constants {
 
         // Photonvision thing
         // The standard deviations of our vision estimated poses, which affect correction rate
-        // (Fake values. Experiment and determine estimation noise on an actual robot.)
+        // X, Y, Theta
+        // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
         public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
         public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
     }

src/main/java/frc4388/robot/Robot.java

@@ -10,7 +10,6 @@ package frc4388.robot;
 import java.util.ArrayList;
 import java.util.Base64;
 import java.util.List;
-import java.util.logging.Level;
 
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.CANBus.CANBusStatus;
@@ -87,9 +86,9 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {   
     m_robotTime.updateTimes();
-    SmartDashboard.putNumber("Time", System.currentTimeMillis());
+    // SmartDashboard.putNumber("Time", System.currentTimeMillis());
-    //System.out.println(m_robotContainer.limelight.isNearSpeaker());
+    
-    //mled.updateLED();
+    m_robotContainer.m_robotLED.update();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic

src/main/java/frc4388/robot/RobotContainer.java

@@ -84,7 +84,7 @@ public class RobotContainer {
     public final RobotMap m_robotMap = new RobotMap();
     
     /* Subsystems */
-    private final LED m_robotLED = new LED();
+    public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
     public final Lidar m_lidar = new Lidar();
     public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -7,21 +7,17 @@
 
 package frc4388.robot.subsystems;
 
-import java.util.logging.Level;
-
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
-import frc4388.utility.Status.ReportLevel;
 
 /**
  * Add your docs here.

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -23,9 +23,12 @@ import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.AutoConstants;
 import frc4388.robot.subsystems.LED;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
 import frc4388.utility.TimesNegativeOne;
+import frc4388.utility.Status.ReportLevel;
 
-public class Elevator extends SubsystemBase {
+public class Elevator extends Subsystem {
   /** Creates a new Elevator. */
   private TalonFX elevatorMotor;
   private TalonFX endeffectorMotor;
@@ -303,4 +306,23 @@ public class Elevator extends SubsystemBase {
       //shuffle the coral with the arm until coral hits beam break
     }
   }
+
+  @Override
+  public String getSubsystemName() {
+    return "Elevator";
+  }
+
+  @Override
+  public void queryStatus() {}
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
+    status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor);
+    status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor);
+
+    return status;
+  }
 }

src/main/java/frc4388/robot/subsystems/LED.java

@@ -9,29 +9,67 @@ package frc4388.robot.subsystems;
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
 
 /**
  * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
  * Driver
  */
-public class LED extends SubsystemBase {
+public class LED extends Subsystem {
 
   private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-  private double mode = LEDConstants.DEFAULT_PATTERN.getValue();
+  private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
 
   public void setMode(LEDPatterns pattern){
-    this.mode = pattern.getValue();
+    this.mode = pattern;
   }
 
   @Override
   public void periodic() {
-    LEDController.set(mode);
+    update();
   }
 
+  public void update() {
+    if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
+
+    if(DriverStation.isDisabled()){
+      LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
+    }else
+      LEDController.set(mode.getValue());
+  }
+
+  @Override
+  public String getSubsystemName() {
+    return "LEDs";
+  }
+
+  @Override
+  public void queryStatus() {
+    SmartDashboard.putString("LED status", mode.name());
+  }
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    if(LEDController.isAlive())
+      status.addReport(ReportLevel.INFO, "LED is CONNECTED");
+    else
+      status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
+    
+    status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
+
+    return status;
+  }
+  
+
 }

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -69,11 +69,15 @@ public class Lidar extends Subsystem {
     @Override
     public Status diagnosticStatus() {
         Status s = new Status();
+
         if(distance == -1){
-            s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED");
+            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
         }else{
-            s.addReport(ReportLevel.INFO, "LIDAR CONNECTED");
+            s.addReport(ReportLevel.INFO, "LiDAR Connected");
         }
+
+        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
+
         return s;
     }
     

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -423,7 +423,7 @@ public class SwerveDrive extends Subsystem {
     public Status diagnosticStatus() {
         Status status = new Status();
 
-        status.addReport(ReportLevel.ERROR,
+        status.addReport(ReportLevel.INFO,
                 "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
 
         return status;

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -36,6 +36,7 @@ import frc4388.robot.Constants.FieldConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
 
 public class Vision extends Subsystem {
 
@@ -311,6 +312,17 @@ public class Vision extends Subsystem {
     public Status diagnosticStatus() {
         Status status = new Status();
 
+        if(cameras[0].isConnected())
+            status.addReport(ReportLevel.INFO, "Left Camera Connected");
+        else
+            status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED");
+
+        if(cameras[1].isConnected())
+            status.addReport(ReportLevel.INFO, "Right Camera Connected");
+        else
+            status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED");
+
+
         return status;
     }