Merge pull request #5 from FRC3636/diagnostics

Add minimal diagnostics

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,13 +91,29 @@ public RobotContainer() {
         }
         DriverStation.silenceJoystickConnectionWarning(Robot.isSimulation());
 
+        startupDiagnostics();
+
         setDefaultCommands();
         configureAutoTab();
         configureButtonBindings();
 
         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);

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