Driver dashboard alerts

src/main/java/org/team1540/robot2024/AlertManager.java

@@ -0,0 +1,81 @@
+package org.team1540.robot2024;
+
+import com.ctre.phoenix6.CANBus;
+import edu.wpi.first.hal.can.CANStatus;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import org.team1540.robot2024.util.Alert;
+
+/**
+ * Class for managing high-level robot alerts, such as low battery and CAN bus errors. Subsystem-specific alerts,
+ * such as motor disconnects, should be handled by their respective subsystems.
+ */
+public class AlertManager {
+    private static AlertManager instance;
+
+    public static AlertManager getInstance() {
+        if (instance == null) instance = new AlertManager();
+        return instance;
+    }
+
+    private static final double lowBatteryDisableTime = 1.5;
+    private static final double lowBatteryVoltageThreshold = 12.0;
+    private static final double canErrorTimeThreshold = 0.5;
+
+    private final Timer disabledTimer = new Timer();
+    private final Timer canInitialErrorTimer = new Timer();
+    private final Timer canErrorTimer = new Timer();
+    private final Timer canivoreErrorTimer = new Timer();
+
+    private int lastRioCanTEC = 0;
+    private int lastRioCanREC = 0;
+    private int lastCanivoreTEC = 0;
+    private int lastCanivoreREC = 0;
+
+    private final Alert canErrorAlert =
+            new Alert("RIO CAN bus error", Alert.AlertType.ERROR);
+    private final Alert canivoreErrorAlert =
+            new Alert("Swerve CAN bus error", Alert.AlertType.ERROR);
+    private final Alert lowBatteryAlert =
+            new Alert("Battery voltage is low", Alert.AlertType.WARNING);
+
+    public void start() {
+        disabledTimer.restart();
+        canErrorTimer.restart();
+        canivoreErrorTimer.restart();
+        canInitialErrorTimer.restart();
+    }
+
+    public void update() {
+        // Update timers
+        CANStatus rioCanStatus = RobotController.getCANStatus();
+        if (rioCanStatus.transmitErrorCount > lastRioCanTEC || rioCanStatus.receiveErrorCount > lastRioCanREC) {
+            canErrorTimer.reset();
+        }
+        lastRioCanTEC = rioCanStatus.transmitErrorCount;
+        lastRioCanREC = rioCanStatus.receiveErrorCount;
+
+        CANBus.CANBusStatus canivoreStatus = CANBus.getStatus(Constants.SwerveConfig.CAN_BUS);
+        if (!canivoreStatus.Status.isOK()
+                || canivoreStatus.TEC > lastCanivoreTEC
+                || canivoreStatus.REC > lastCanivoreREC) {
+            canivoreErrorTimer.reset();
+        }
+        lastCanivoreTEC = canivoreStatus.TEC;
+        lastCanivoreREC = canivoreStatus.REC;
+
+        if (DriverStation.isEnabled()) disabledTimer.reset();
+
+        canErrorAlert.set(
+                !canErrorTimer.hasElapsed(canErrorTimeThreshold)
+                        && canInitialErrorTimer.hasElapsed(canErrorTimeThreshold));
+        canivoreErrorAlert.set(
+                !canivoreErrorTimer.hasElapsed(canErrorTimeThreshold)
+                        && canInitialErrorTimer.hasElapsed(canErrorTimeThreshold));
+        lowBatteryAlert.setText("Battery voltage is low (" + RobotController.getBatteryVoltage() + "V)");
+        lowBatteryAlert.set(
+                RobotController.getBatteryVoltage() < lowBatteryVoltageThreshold
+                        && disabledTimer.hasElapsed(lowBatteryDisableTime));
+    }
+}

src/main/java/org/team1540/robot2024/Robot.java

@@ -92,6 +92,8 @@ public void robotInit() {
         // Start AdvantageKit logger
         Logger.start();
 
+        AlertManager.getInstance().start();
+
         DriverStation.silenceJoystickConnectionWarning(true);
 
         // Instantiate our RobotContainer. This will perform all our button bindings,
@@ -122,6 +124,7 @@ public void robotPeriodic() {
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
         if (Constants.currentMode == Constants.Mode.REAL) robotContainer.odometrySignalRefresher.periodic();
+        AlertManager.getInstance().update();
 
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -30,6 +30,7 @@
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.vision.AprilTagVision;
+import org.team1540.robot2024.util.Alert;
 import org.team1540.robot2024.util.CommandUtils;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.auto.AutoCommand;
@@ -56,8 +57,6 @@ public class RobotContainer {
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
-
-
     public boolean isBrakeMode;
     /**
      * The container for the robot. Contains subsystems, IO devices, and commands.
@@ -242,26 +241,25 @@ private void configureAutoRoutines() {
         AutoManager autos = AutoManager.getInstance();
         // Set up FF characterization routines
         if (isTuningMode()) {
-            AutoManager.getInstance().add(
-                    new AutoCommand(
-                            "Drive FF Characterization",
-                            new FeedForwardCharacterization(
-                                    drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity
-                            )
-                    )
-            );
-
-            AutoManager.getInstance().add(
-                    new AutoCommand(
-                            "Flywheels FF Characterization",
-                            new FeedForwardCharacterization(
-                                    shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60
-                            )
-                    )
-            );
-
+            new Alert("Tuning mode enabled", Alert.AlertType.INFO).set(true);
+            autos.add(new AutoCommand(
+                    "Drive FF Characterization",
+                    new FeedForwardCharacterization(
+                            drivetrain,
+                            drivetrain::runCharacterizationVolts,
+                            drivetrain::getCharacterizationVelocity)));
+            autos.add(new AutoCommand(
+                    "Flywheels FF Characterization",
+                    new FeedForwardCharacterization(
+                            shooter,
+                            volts -> shooter.setFlywheelVolts(volts, volts),
+                            () -> shooter.getLeftFlywheelSpeed() / 60)));
+            autos.add(new AutoCommand(
+                    "WheelRadiusChar",
+                    new WheelRadiusCharacterization(
+                            drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE)));
         }
-        autos.add(new AutoCommand("WheelRadiusChar", new WheelRadiusCharacterization(drivetrain, WheelRadiusCharacterization.Direction.COUNTER_CLOCKWISE)));
+
         autos.addDefault(new AutoCommand("Dwayne :skull:"));
         autos.add(new AmpLanePADEF(drivetrain, shooter, indexer));
         autos.add(new AmpLanePAEDF(drivetrain, shooter, indexer));

src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

@@ -1,7 +1,6 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.commands.FollowPathHolonomic;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
@@ -28,6 +27,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.Robot;
+import org.team1540.robot2024.util.Alert;
 import org.team1540.robot2024.util.auto.LocalADStarAK;
 import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
@@ -64,6 +65,8 @@ public class Drivetrain extends SubsystemBase {
 
     private Pose2d targetPose = new Pose2d();
 
+    private final Alert gyroDisconnected = new Alert("Gyro disconnected!", Alert.AlertType.WARNING);
+
     private Drivetrain(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -193,6 +196,7 @@ public void periodic() {
         poseEstimator.update(rawGyroRotation, getModulePositions());
         visionPoseEstimator.update(rawGyroRotation, getModulePositions());
 
+        gyroDisconnected.set(!gyroInputs.connected && Robot.isReal());
     }
 
     /**

src/main/java/org/team1540/robot2024/subsystems/drive/Module.java

@@ -7,11 +7,14 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.util.Alert;
 
 import static org.team1540.robot2024.Constants.Drivetrain.MAX_LINEAR_SPEED;
 import static org.team1540.robot2024.Constants.Drivetrain.WHEEL_RADIUS;
 
 public class Module {
+    private static final String[] moduleNames = new String[]{"FL", "FR", "BL", "BR"};
+
     private final ModuleIO io;
     private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
     private final int index;
@@ -23,6 +26,10 @@ public class Module {
     private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
     private double lastPositionMeters = 0.0; // Used for delta calculation
 
+    private final Alert driveMotorDisconnected;
+    private final Alert turnMotorDisconnected;
+    private final Alert turnEncoderDisconnected;
+
     public Module(ModuleIO io, int index) {
         this.io = io;
         this.index = index;
@@ -33,7 +40,6 @@ public Module(ModuleIO io, int index) {
             case REAL:
             case REPLAY:
                 driveFeedforward = new SimpleMotorFeedforward(0.258, 0.128);
-//                driveFeedforward = new SimpleMotorFeedforward(0.206, 0.117);
                 driveFeedback = new PIDController(0.05, 0.0, 0.0);
                 turnFeedback = new PIDController(7.0, 0.0, 0.0);
                 break;
@@ -51,6 +57,14 @@ public Module(ModuleIO io, int index) {
 
         turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
         setBrakeMode(true);
+
+        driveMotorDisconnected =
+                new Alert(moduleNames[index] + " drive motor disconnected!", Alert.AlertType.WARNING);
+        turnMotorDisconnected =
+                new Alert(moduleNames[index] + " turn motor disconnected!", Alert.AlertType.WARNING);
+        turnEncoderDisconnected =
+                new Alert(moduleNames[index] + " turn encoder disconnected!", Alert.AlertType.WARNING);
+
     }
 
     public void periodic() {
@@ -79,6 +93,10 @@ public void periodic() {
                                 + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec));
             }
         }
+
+        driveMotorDisconnected.set(!inputs.driveMotorConnected);
+        turnMotorDisconnected.set(!inputs.turnMotorConnected);
+        turnEncoderDisconnected.set(!inputs.turnEncoderConnected);
     }
 
     public SwerveModuleState runSetpoint(SwerveModuleState state) {

src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java

@@ -11,13 +11,16 @@ class ModuleIOInputs {
         public double driveAppliedVolts = 0.0;
         public double driveCurrentAmps = 0.0;
         public double driveTempCelsius = 0.0;
+        public boolean driveMotorConnected = true;
 
         public Rotation2d turnAbsolutePosition = new Rotation2d();
         public Rotation2d turnPosition = new Rotation2d();
         public double turnVelocityRadPerSec = 0.0;
         public double turnAppliedVolts = 0.0;
         public double turnCurrentAmps = 0.0;
         public double turnTempCelsius = 0.0;
+        public boolean turnMotorConnected = true;
+        public boolean turnEncoderConnected = true;
     }
 
     /**

src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java

@@ -92,16 +92,17 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw, PhoenixTimeSyncSignalRef
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
         odometrySignalRefresher.refreshSignals();
-        BaseStatusSignal.refreshAll(
+        inputs.driveMotorConnected = BaseStatusSignal.refreshAll(
                 driveVelocity,
                 driveAppliedVolts,
                 driveCurrent,
-                driveTempCelsius,
-                turnAbsolutePosition,
+                driveTempCelsius).isOK();
+        inputs.turnMotorConnected = BaseStatusSignal.refreshAll(
                 turnVelocity,
                 turnAppliedVolts,
                 turnCurrent,
-                turnTempCelsius);
+                turnTempCelsius).isOK();
+        inputs.turnEncoderConnected = BaseStatusSignal.refreshAll(turnAbsolutePosition).isOK();
 
         inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO;
         inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO;

src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java

@@ -7,6 +7,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.util.Alert;
 import org.team1540.robot2024.util.LoggedTunableNumber;
 import org.team1540.robot2024.util.MechanismVisualiser;
 import org.team1540.robot2024.util.math.AverageFilter;
@@ -24,6 +25,11 @@ public class Elevator extends SubsystemBase {
     private final LoggedTunableNumber kI = new LoggedTunableNumber("Elevator/kI", KI);
     private final LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD", KD);
 
+    private final Alert leadMotorDisconnected =
+            new Alert("Elevator lead motor disconnected!", Alert.AlertType.WARNING);
+    private final Alert followMotorDisconnected =
+            new Alert("Elevator follower motor disconnected!", Alert.AlertType.WARNING);
+
     private static boolean hasInstance = false;
 
     private Elevator(ElevatorIO elevatorIO) {
@@ -69,6 +75,9 @@ public void periodic() {
         if(getPosition() > 0.31){
             setFlipper(false);
         }
+
+        leadMotorDisconnected.set(!inputs.leadMotorConnected);
+        followMotorDisconnected.set(!inputs.followMotorConnected);
     }
 
     public void setElevatorPosition(double positionMeters) {

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java

@@ -13,6 +13,9 @@ class ElevatorIOInputs {
         public boolean atUpperLimit = false;
         public boolean atLowerLimit = false;
         public double flipperAngleDegrees = 0.0;
+
+        public boolean leadMotorConnected = true;
+        public boolean followMotorConnected = true;
     }
 
     default void updateInputs(ElevatorIOInputs inputs) {}

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java

@@ -28,11 +28,10 @@ public class ElevatorIOTalonFX implements ElevatorIO {
     private final StatusSignal<Double> followerTemp = follower.getDeviceTemp();
     private final StatusSignal<ForwardLimitValue> topLimitStatus = leader.getForwardLimit();
     private final StatusSignal<ReverseLimitValue> bottomLimitStatus = leader.getReverseLimit();
-    TalonFXConfiguration config;
 
 
     public ElevatorIOTalonFX() {
-        config = new TalonFXConfiguration();
+        TalonFXConfiguration config = new TalonFXConfiguration();
         config.CurrentLimits.SupplyCurrentLimitEnable = true;
         config.CurrentLimits.SupplyCurrentLimit = 40.0;
         config.CurrentLimits.SupplyCurrentThreshold = 60.0;
@@ -83,16 +82,15 @@ public ElevatorIOTalonFX() {
 
     @Override
     public void updateInputs(ElevatorIOInputs inputs) {
-        BaseStatusSignal.refreshAll(
+        inputs.leadMotorConnected = BaseStatusSignal.refreshAll(
                 leaderPosition,
                 leaderVelocity,
                 leaderAppliedVolts,
                 leaderCurrent,
-                followerCurrent,
                 leaderTemp,
-                followerTemp,
                 topLimitStatus,
-                bottomLimitStatus);
+                bottomLimitStatus).isOK();
+        inputs.followMotorConnected = BaseStatusSignal.refreshAll(followerCurrent, followerTemp).isOK();
 
         inputs.positionMeters = leaderPosition.getValueAsDouble();
         inputs.velocityMPS = leaderVelocity.getValueAsDouble();

src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java

@@ -9,6 +9,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.util.Alert;
 import org.team1540.robot2024.util.LoggedTunableNumber;
 
 import static org.team1540.robot2024.Constants.Indexer.*;
@@ -22,6 +23,9 @@ public class Indexer extends SubsystemBase {
     private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
     private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
 
+    private final Alert intakeMotorDisconnected = new Alert("Intake motor disconnected!", Alert.AlertType.WARNING);
+    private final Alert feederMotorDisconnected = new Alert("Feeder motor disconnected!", Alert.AlertType.WARNING);
+
     private double feederSetpointRPM = 0.0;
 
     private static boolean hasInstance = false;
@@ -58,9 +62,12 @@ public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Indexer", inputs);
 
-        if (RobotState.isDisabled()){
+        if (RobotState.isDisabled()) {
             stopAll();
         }
+
+        intakeMotorDisconnected.set(!inputs.intakeMotorConnected);
+        feederMotorDisconnected.set(!inputs.feederMotorConnected);
     }
 
     public void setIntakePercent(double percent) {