From c183c08a3d92ff8561bef96dad52b8dd64d94f14 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 11 Jul 2025 17:58:22 -0600 Subject: [PATCH] Slightly overhaul status system --- src/main/java/frc4388/robot/Robot.java | 99 +--------- src/main/java/frc4388/robot/RobotMap.java | 33 +++- .../robot/constants/BuildConstants.java | 10 +- .../robot/constants/DriveConstants.java | 1 - .../frc4388/robot/subsystems/DiffDrive.java | 12 +- .../frc4388/robot/subsystems/Elevator.java | 12 +- .../java/frc4388/robot/subsystems/LED.java | 15 +- .../java/frc4388/robot/subsystems/Lidar.java | 19 +- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +- .../java/frc4388/robot/subsystems/Vision.java | 39 +--- .../frc4388/utility/status/FaultCANCoder.java | 82 +++++++++ .../utility/status/FaultPhotonCamera.java | 38 ++++ .../frc4388/utility/status/FaultPidgeon2.java | 174 ++++++++++++++++++ .../frc4388/utility/status/FaultReporter.java | 129 +++++++++++++ .../frc4388/utility/status/FaultTalonFX.java | 160 ++++++++++++++++ .../frc4388/utility/status/QueryUtils.java | 13 ++ .../frc4388/utility/status/Queryable.java | 10 + .../java/frc4388/utility/status/Status.java | 4 + .../frc4388/utility/status/Subsystem.java | 25 --- 19 files changed, 698 insertions(+), 194 deletions(-) create mode 100644 src/main/java/frc4388/utility/status/FaultCANCoder.java create mode 100644 src/main/java/frc4388/utility/status/FaultPhotonCamera.java create mode 100644 src/main/java/frc4388/utility/status/FaultPidgeon2.java create mode 100644 src/main/java/frc4388/utility/status/FaultReporter.java create mode 100644 src/main/java/frc4388/utility/status/FaultTalonFX.java create mode 100644 src/main/java/frc4388/utility/status/QueryUtils.java create mode 100644 src/main/java/frc4388/utility/status/Queryable.java delete mode 100644 src/main/java/frc4388/utility/status/Subsystem.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 6b948bf..bb38c33 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,10 +7,6 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.Base64; -import java.util.List; - // Advantagekit import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -19,23 +15,16 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.CANBus.CANBusStatus; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; -import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; import frc4388.utility.DeferredBlockMulti; import frc4388.utility.Trim; import frc4388.utility.compute.RobotTime; -import frc4388.utility.status.CanDevice; -import frc4388.utility.status.Status; -import frc4388.utility.status.Subsystem; -import frc4388.utility.status.Status.Report; -import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.status.FaultReporter; + //import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the @@ -63,23 +52,7 @@ public class Robot extends LoggedRobot { // Create a shuffleboard update thread, that will periodically update the values on shuffleboard - new Thread() { - public void run() { - try{ - while(!this.isInterrupted() && this.isAlive()){ - Thread.sleep(500); - for(int i=0;i errors = new ArrayList<>(); - - // Subsystems header - System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY"))); - - for(int i=0;i< Subsystem.subsystems.size();i++){ - - Subsystem subsystem = Subsystem.subsystems.get(i); - System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":"); - Status status = subsystem.diagnosticStatus(); - - for(int a=0;a 0) { - // Errors header - System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY"))); - for(int i=0;i swerveDriveTrain; private Vision vision; @@ -65,7 +66,7 @@ public class SwerveDrive extends Subsystem { public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain // swerveDriveTrain) { - super(); + FaultReporter.register(this); this.swerveDriveTrain = swerveDriveTrain; this.vision = vision; @@ -448,12 +449,12 @@ public class SwerveDrive extends Subsystem { @Override - public String getSubsystemName() { + public String getName() { return "Swerve Drive Controller"; } ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .getLayout(getName(), BuiltInLayouts.kList) .withSize(2, 2); GenericEntry sbGyro = subsystemLayout @@ -478,8 +479,8 @@ public class SwerveDrive extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); - 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."); + // 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; } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index d6211a0..5a85772 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -27,13 +27,14 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; import frc4388.utility.status.Status; -import frc4388.utility.status.Subsystem; -import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; -public class Vision extends Subsystem { +public class Vision extends SubsystemBase implements Queryable { // private PhotonCamera leftCamera; // private PhotonCamera rightCamera; @@ -59,7 +60,7 @@ public class Vision extends Subsystem { private Field2d field = new Field2d(); ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .getLayout(getName(), BuiltInLayouts.kList) .withSize(2, 2); GenericEntry sbTagDetected = subsystemLayout @@ -72,17 +73,8 @@ public class Vision extends Subsystem { .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - GenericEntry sbLeftCamConnected = subsystemLayout - .add("Left Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbRightCamConnected = subsystemLayout - .add("Right Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ + FaultReporter.register(this); SmartDashboard.putData(field); this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; @@ -307,7 +299,7 @@ public class Vision extends Subsystem { @Override - public String getSubsystemName() { + public String getName() { return "Vision"; } @@ -321,27 +313,12 @@ public class Vision extends Subsystem { public void queryStatus() { sbTagDetected.setBoolean(isTagDetected); sbTagProcessed.setBoolean(isTagProcessed); - sbLeftCamConnected.setBoolean(cameras[0].isConnected()); - sbRightCamConnected.setBoolean(cameras[1].isConnected()); // field.setRobotPose(getPose2d()); } @Override 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; + return new Status(); } } diff --git a/src/main/java/frc4388/utility/status/FaultCANCoder.java b/src/main/java/frc4388/utility/status/FaultCANCoder.java new file mode 100644 index 0000000..ad11ad6 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultCANCoder.java @@ -0,0 +1,82 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.CANcoder; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultCANCoder implements Queryable { + private String name; + private CANcoder cancoder; + + public static void addDevice(CANcoder cancoder, String name) { + FaultCANCoder p = new FaultCANCoder(); + + p.name = name; + p.cancoder = cancoder; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() { + // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage()); + boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + // faults + if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Bad magnet"); + } + if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet"); + } + if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout"); + } + if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java new file mode 100644 index 0000000..0b35cb5 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java @@ -0,0 +1,38 @@ +package frc4388.utility.status; + +import org.photonvision.PhotonCamera; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPhotonCamera implements Queryable { + private String name; + private PhotonCamera cam; + + public static void addDevice(PhotonCamera cam, String name) { + FaultPhotonCamera p = new FaultPhotonCamera(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(!cam.isConnected()) + s.addReport(ReportLevel.ERROR, "Not Connected!"); + + return s; + } +} + diff --git a/src/main/java/frc4388/utility/status/FaultPidgeon2.java b/src/main/java/frc4388/utility/status/FaultPidgeon2.java new file mode 100644 index 0000000..a6f0b81 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPidgeon2.java @@ -0,0 +1,174 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.Pigeon2; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPidgeon2 implements Queryable { + private String name; + private Pigeon2 pigeon2; + + public static void addDevice(Pigeon2 pigeon2, String name) { + FaultPidgeon2 p = new FaultPidgeon2(); + + p.name = name; + p.pigeon2 = pigeon2; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() { + // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + + + boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage()); + boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + + s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage()); + + s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch()); + s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw()); + s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll()); + + s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX()); + s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY()); + s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ()); + + s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX()); + s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY()); + s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ()); + + + // faults + if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while in motion"); + } + if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Accelerometer fault detected"); + } + if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro fault detected"); + } + if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Magnetometer fault detected"); + } + if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack data acquisition slower than expected"); + } + if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack loop time was slower than expected"); + } + if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Accelerometer values are saturated"); + } + if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro values are saturated"); + } + if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Magnetometer values are saturated"); + } + if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while in motion"); + } + if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Accelerometer fault detected"); + } + if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected"); + } + if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Magnetometer fault detected"); + } + if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack data acquisition slower than expected")); + } + if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack loop time was slower than expected")); + } + if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Accelerometer values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Gyro values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Magnetometer values are saturated"); + } + if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Device supply voltage near brownout"); + } + if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java new file mode 100644 index 0000000..853c727 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -0,0 +1,129 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; + +import frc4388.robot.constants.Constants; +import frc4388.utility.status.Status.Report; +import frc4388.utility.status.Status.ReportLevel; + +public class FaultReporter { + + private static final String REPORTS_HEADER = + "▄▖ ▗ \n" + // + "▙▘█▌▛▌▛▌▛▘▜▘▛▘\n" + // + "▌▌▙▖▙▌▙▌▌ ▐▖▄▌\n" + // + " ▌ "; + + private static final String CAN_HEADER = + "▄▖▄▖▖ ▖ \n" + // + "▌ ▌▌▛▖▌ \n" + // + "▙▖▛▌▌▝▌(t)\n" + // + " "; + + private static final String ERROR_HEADER = + "▄▖▄▖▄▖▄▖▄▖▄▖\n" + // + "▙▖▙▘▙▘▌▌▙▘▚ \n" + // + "▙▖▌▌▌▌▙▌▌▌▄▌\n" + // + " "; + + private static List queryables = new ArrayList<>(); + + public static void startThread() { + new Thread() { + public void run() { + try{ + while(!this.isInterrupted() && this.isAlive()){ + Thread.sleep(500); + for(int i=0;i errors = new ArrayList<>(); + + // Subsystems header + System.out.println(REPORTS_HEADER); + + for(int i=0;i< queryables.size();i++){ + + Queryable q = queryables.get(i); + System.out.println("** Subsystem diagnostic report for " + q.getName() + ":"); + Status status = q.diagnosticStatus(); + + for(int a=0;a 0) { + // Errors header + System.out.println(ERROR_HEADER); + for(int i=0;i subsystems = new ArrayList<>(); - - public Subsystem(){ - subsystems.add(this); - } - - public void Log(String str) { - System.out.println(getSubsystemName() + " - " + str); - } - - // Get name of subsystem, for use in log. - public abstract String getSubsystemName(); - // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard - public abstract void queryStatus(); - // Proactivly search for any errors in each subsystem - public abstract Status diagnosticStatus(); -}