State Check

Team3128 · Feb 10, 2024 · 52cd394 · 52cd394
1 parent 30e38a8
commit 52cd394
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 2 deletions.
diff --git a/src/main/java/common/core/swerve/SwerveBase.java b/src/main/java/common/core/swerve/SwerveBase.java
@@ -3,6 +3,7 @@
 import org.littletonrobotics.junction.Logger;
 
 import common.hardware.motorcontroller.NAR_Motor.Control;
+import common.utility.narwhaldashboard.NarwhalDashboard;
 import common.utility.shuffleboard.NAR_Shuffleboard;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -61,8 +62,8 @@ public void initShuffleboard() {
             NAR_Shuffleboard.addData("Swerve", "Drive Motor" + module.moduleNumber, ()-> module.getState().speedMetersPerSecond, 2, module.moduleNumber);
         }
         NAR_Shuffleboard.addData("Swerve", "Pose", ()-> estimatedPose.toString(), 3, 0, 4, 1);
-        NAR_Shuffleboard.addData("Swerve", "Robot Velocity", getRobotVelocity().toString(), 3, 1, 4, 1);
-        NAR_Shuffleboard.addData("Swerve", "Velocity", getVelocity(), 3, 3, 1, 1);
+        NAR_Shuffleboard.addData("Swerve", "Robot Velocity", ()-> getRobotVelocity().toString(), 3, 1, 4, 1);
+        NAR_Shuffleboard.addData("Swerve", "Velocity", ()-> getVelocity(), 3, 3, 1, 1);
         NAR_Shuffleboard.addData("Swerve", "Field Velocity", ()-> getFieldVelocity().toString(), 3, 2, 4, 1);
         NAR_Shuffleboard.addData("Swerve", "Gyro", ()-> getYaw(), 7, 0, 2, 2).withWidget("Gyro");
     }
@@ -253,4 +254,10 @@ public double getVelocity() {
     public SwerveModule[] getModules() {
         return modules;
     }
+
+    public void initStateCheck() {
+        for (final SwerveModule module : modules) {
+            NarwhalDashboard.getInstance().checkState("Module " + module.moduleNumber, ()-> module.getRunningState());
+        }
+    }
 }
diff --git a/src/main/java/common/core/swerve/SwerveModule.java b/src/main/java/common/core/swerve/SwerveModule.java
@@ -15,6 +15,7 @@
 import common.hardware.motorcontroller.NAR_Motor;
 import common.hardware.motorcontroller.NAR_Motor.Control;
 import common.hardware.motorcontroller.NAR_Motor.Neutral;
+import common.utility.narwhaldashboard.NarwhalDashboard.State;
 
 
 /**
@@ -216,4 +217,14 @@ public NAR_Motor getDriveMotor() {
     public NAR_Motor getAngleMotor() {
         return angleMotor;
     }
+
+    public State getRunningState() {
+        if (driveMotor.getVelocity() != 0 && angleMotor.getVelocity() != 0) {
+            return State.RUNNING; 
+        }
+        if (driveMotor.getVelocity() != 0 || angleMotor.getVelocity() != 0) {
+            return State.PARTIALLY_RUNNING; 
+        }
+        return State.DISCONNECTED;
+    }
 }
diff --git a/src/main/java/common/utility/narwhaldashboard/NarwhalDashboard.java b/src/main/java/common/utility/narwhaldashboard/NarwhalDashboard.java
@@ -40,6 +40,12 @@ public class NarwhalDashboard extends WebSocketServer implements AutoCloseable {
 
     private static int PORT = 5805;
 
+    public enum State {
+        RUNNING,
+        PARTIALLY_RUNNING,
+        DISCONNECTED
+    }
+
     public static synchronized NarwhalDashboard getInstance() {
         if (instance == null) {
             startServer();
@@ -265,6 +271,10 @@ private void updateButton(String key, boolean down) {
         buttons.get(key).accept(down);
     }
 
+    public void checkState(String name, Supplier<State> state) {
+        addUpdate(name, ()-> state.get());
+    }
+
     @Override
     public void onError(WebSocket conn, Exception ex) {
         ex.printStackTrace();