From 52cd3949155b8ae21bf4ac8e6297411d4525a0a9 Mon Sep 17 00:00:00 2001
From: Audrey <audrey3zheng@gmail.com>
Date: Sat, 10 Feb 2024 13:54:54 -0800
Subject: [PATCH] State Check

---
 src/main/java/common/core/swerve/SwerveBase.java      | 11 +++++++++--
 src/main/java/common/core/swerve/SwerveModule.java    | 11 +++++++++++
 .../utility/narwhaldashboard/NarwhalDashboard.java    | 10 ++++++++++
 3 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/src/main/java/common/core/swerve/SwerveBase.java b/src/main/java/common/core/swerve/SwerveBase.java
index 09afa93..6ddd60e 100644
--- 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());
+        }
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/common/core/swerve/SwerveModule.java b/src/main/java/common/core/swerve/SwerveModule.java
index c7de818..b8d34f2 100644
--- 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;
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/common/utility/narwhaldashboard/NarwhalDashboard.java b/src/main/java/common/utility/narwhaldashboard/NarwhalDashboard.java
index 1b0d5dc..17b88aa 100644
--- 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();