diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7340da7..a94468d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,15 +37,16 @@ import frc.robot.utils.AutoLanguage; import frc.robot.utils.GamePiece; import frc.robot.utils.Node; - import java.util.Optional; + public class RobotContainer { // Dashboard public static final ShuffleboardTab driveSettingsTab = Shuffleboard.getTab("Drive Settings"); public static final ShuffleboardTab autoTab = Shuffleboard.getTab("Auto"); public static final ShuffleboardTab swerveTab = Shuffleboard.getTab("Swerve"); public static final ShuffleboardTab armTab = Shuffleboard.getTab("Arm"); + public static final ShuffleboardTab diagnosticsTab = Shuffleboard.getTab("Diagnostics"); // Subsystems public static final Drivetrain drivetrain = new Drivetrain(); @@ -90,6 +91,8 @@ public RobotContainer() { } DriverStation.silenceJoystickConnectionWarning(Robot.isSimulation()); + startupDiagnostics(); + setDefaultCommands(); configureAutoTab(); configureButtonBindings(); @@ -97,6 +100,20 @@ public RobotContainer() { enableLogging(); } + + // This was suggested by Ron just so if we're at a competition we don't make silly mistakes like forgetting to swap the battery. + // Hopefully next season we can add things like checking to make sure everything on the CAN bus is responding and stuff like that. + // The intention is to avoid an "oh crap I forgot to plug that CAN wire back in and we're going on the field" moment. + // TODO: Add a command where we can re-run this diagnostics check at any time. + private void startupDiagnostics() { + // Check battery voltage + // Note: The 12.1 value is just an arbitrary value should probably be changed + diagnosticsTab.addBoolean("Battery Test Pass", () -> (RobotController.getBatteryVoltage() < 12.1)); + + diagnosticsTab.addBoolean("NavX Connection Test Pass", () -> drivetrain.getNavXStatus()); + + } + private void enableLogging() { LiveWindow.enableTelemetry(arm); LiveWindow.enableTelemetry(drivetrain); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 3cde72c..e7d3d6d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -231,4 +231,11 @@ public double getTurnRate() { return gyro.getRate().getDegrees(); } + public Boolean getNavXStatus() { + if (RobotBase.isSimulation()) + return false; + + return navXGyro.isConnected(); + } + }