feat: NT alerts for motor and camera disconnects

flamingchickens1540 · May 20, 2024 · 323973b · 323973b
1 parent f050981
commit 323973b
Show file tree

Hide file tree

Showing 23 changed files with 276 additions and 21 deletions.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/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,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
+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 +64,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 +195,7 @@ public void periodic() {
         poseEstimator.update(rawGyroRotation, getModulePositions());
         visionPoseEstimator.update(rawGyroRotation, getModulePositions());
 
+        gyroDisconnected.set(!gyroInputs.connected);
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/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) {

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIO.java b/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;
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/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;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/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) {

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/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) {}

diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/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();

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/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) {

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -10,10 +10,14 @@ class IndexerIOInputs {
         public double intakeCurrentAmps = 0.01;
         public double intakeVelocityRPM = 0.0;
         public double intakeTempCelsius = 0.0;
+        public boolean intakeMotorConnected = true;
+
         public double feederVoltage = 0.0;
         public double feederCurrentAmps = 0.0;
         public double feederVelocityRPM = 0.0;
         public double feederTempCelsius = 0.0;
+        public boolean feederMotorConnected = true;
+
         public boolean noteInIntake = false;
     }
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOTalonFX.java
@@ -70,15 +70,16 @@ public IndexerIOTalonFX() {
 
     @Override
     public void updateInputs(IndexerIOInputs inputs) {
-        BaseStatusSignal.refreshAll(
+        inputs.intakeMotorConnected = BaseStatusSignal.refreshAll(
                 intakeVoltage,
                 intakeCurrent,
                 intakeVelocity,
-                intakeTemp,
+                intakeTemp).isOK();
+        inputs.feederMotorConnected = BaseStatusSignal.refreshAll(
                 feederVoltage,
                 feederCurrent,
                 feederVelocity,
-                feederTemp);
+                feederTemp).isOK();
         inputs.intakeVoltage = intakeVoltage.getValueAsDouble();
         inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble();
         inputs.intakeVelocityRPM = intakeVelocity.getValueAsDouble() * 60;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java
@@ -9,11 +9,13 @@ class FlywheelsIOInputs {
         public double leftCurrentAmps = 0.0;
         public double leftVelocityRPM = 0.0;
         public double leftTempCelsius = 0.0;
+        public boolean leftMotorConnected = true;
 
         public double rightAppliedVolts = 0.0;
         public double rightCurrentAmps = 0.0;
         public double rightVelocityRPM = 0.0;
         public double rightTempCelsius = 0.0;
+        public boolean rightMotorConnected = true;
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java
@@ -78,15 +78,16 @@ public FlywheelsIOTalonFX() {
 
     @Override
     public void updateInputs(FlywheelsIOInputs inputs) {
-        BaseStatusSignal.refreshAll(
+        inputs.leftMotorConnected = BaseStatusSignal.refreshAll(
                 leftVelocity,
                 leftAppliedVolts,
                 leftCurrent,
-                leftTempCelsius,
+                leftTempCelsius).isOK();
+        inputs.rightMotorConnected = BaseStatusSignal.refreshAll(
                 rightVelocity,
                 rightAppliedVolts,
                 rightCurrent,
-                rightTempCelsius);
+                rightTempCelsius).isOK();
 
         inputs.leftVelocityRPM = leftVelocity.getValueAsDouble() * 60;
         inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble();

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/Shooter.java
@@ -11,6 +11,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;
@@ -33,6 +34,11 @@ public class Shooter extends SubsystemBase {
     private final AverageFilter rightSpeedFilter = new AverageFilter(20); // Units: RPM
     private final AverageFilter pivotPositionFilter = new AverageFilter(10); // Units: rotations
 
+    private final Alert leftFlywheelDisconnected = new Alert("Left flywheel disconnected!", Alert.AlertType.WARNING);
+    private final Alert rightFlywheelDisconnected = new Alert("Right flywheel disconnected!", Alert.AlertType.WARNING);
+    private final Alert pivotMotorDisconnected = new Alert("Pivot motor disconnected!", Alert.AlertType.WARNING);
+    private final Alert pivotEncoderDisconnected = new Alert("Pivot encoder disconnected!", Alert.AlertType.WARNING);
+
     private double leftFlywheelSetpointRPM;
     private double rightFlywheelSetpointRPM;
     private Rotation2d pivotSetpoint = new Rotation2d();
@@ -126,6 +132,11 @@ public void periodic() {
         pivotPositionFilter.add(getPivotPosition().getRotations());
         Logger.recordOutput("Shooter/Pivot/Error", pivotSetpoint.getDegrees() - pivotInputs.position.getDegrees());
         Logger.recordOutput("Shooter/Pivot/ResettingToCancoder", flipper);
+
+        leftFlywheelDisconnected.set(!flywheelInputs.leftMotorConnected);
+        rightFlywheelDisconnected.set(!flywheelInputs.rightMotorConnected);
+        pivotMotorDisconnected.set(!pivotInputs.motorConnected);
+        pivotEncoderDisconnected.set(!pivotInputs.encoderConnected);
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java
@@ -14,6 +14,8 @@ class ShooterPivotIOInputs {
         public double tempCelsius = 0.0;
         public boolean isAtForwardLimit = false;
         public boolean isAtReverseLimit = false;
+        public boolean motorConnected = true;
+        public boolean encoderConnected = true;
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java
@@ -95,7 +95,15 @@ public ShooterPivotIOTalonFX() {
 
     @Override
     public void updateInputs(ShooterPivotIOInputs inputs) {
-        BaseStatusSignal.refreshAll(position, absolutePosition, velocity, appliedVoltage, current, temp, forwardLimit, reverseLimit);
+        inputs.motorConnected = BaseStatusSignal.refreshAll(
+                position,
+                velocity,
+                appliedVoltage,
+                current,
+                temp,
+                forwardLimit,
+                reverseLimit).isOK();
+        inputs.encoderConnected = BaseStatusSignal.refreshAll(absolutePosition).isOK();
         inputs.isAtForwardLimit = forwardLimit.getValue() == ForwardLimitValue.ClosedToGround;
         inputs.isAtReverseLimit = reverseLimit.getValue() == ReverseLimitValue.ClosedToGround;
         inputs.position = Rotation2d.fromRotations(position.getValueAsDouble());

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIO.java
@@ -12,6 +12,7 @@ class TrampIOInputs {
         public double currentAmps = 0.0;
         public double positionRots = 0.0;
         public double tempCelsius = 0.0;
+        public boolean motorConnected = true;
     }
 
     default void setVoltage(double volts) {}

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOTalonFX.java
@@ -44,7 +44,7 @@ public void setVoltage(double volts) {
 
     @Override
     public void updateInputs(TrampIOInputs inputs) {
-        BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, current, temp);
+        inputs.motorConnected = BaseStatusSignal.refreshAll(position, velocity, appliedVoltage, current, temp).isOK();
         inputs.noteInTramp = !(beamBreak.get());
         inputs.velocityRPM = velocity.getValueAsDouble() * 60;
         inputs.positionRots = position.getValueAsDouble();