From d508822d1946f39c7a8a8a12af2125a52182ce93 Mon Sep 17 00:00:00 2001
From: C4llSiqn <zachwilke@icloud.com>
Date: Thu, 28 Nov 2024 18:12:17 -0700
Subject: [PATCH] add diagonostic for swevedrive

---
 .../frc4388/robot/subsystems/SwerveDrive.java | 32 +++++++++++--------
 1 file changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 636d9b0..0f9d8e4 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -21,6 +21,7 @@ import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotUnits;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
+import frc4388.utility.Status.Report;
 import frc4388.utility.Status.ReportLevel;
 
 public class SwerveDrive extends Subsystem {
@@ -329,26 +330,31 @@ public class SwerveDrive extends Subsystem {
 
   @Override
   public String getSubsystemName() {
-      return "Swerve Drive Controller";
+    return "Swerve Drive Controller";
   }
 
   @Override
   public Status queryStatus() {
-      Status status = new Status();
-      
-      status.addReport(ReportLevel.INFO, "Gyro Angle: " + this.gyro.getAngle());
-      status.addReport(ReportLevel.INFO, "Shift State: " + this.speedAdjust);
-      //TODO: Add more status things
-      
-      return status;
+    Status status = new Status();
+    
+    status.addReport(ReportLevel.INFO, "Gyro Angle: " + this.gyro.getAngle());
+    status.addReport(ReportLevel.INFO, "Shift State: " + this.speedAdjust);
+    //TODO: Add more status things
+    
+    return status;
   }
 
   @Override
   public Status diagnosticStatus() {
-      Log("Diagnostic info for this has not been inplemented!"); //TODO
-      return new Status();
+    Status status = new Status();
+    for (SwerveModule module : modules) {
+      for (Report moduleDignostic : module.diagnosticStatus().reports) {
+        status.addReport(moduleDignostic.reportLevel, "[" + module.getSubsystemName() + "] " + moduleDignostic.description);
+      }
+    }
+
+    status.diagnoseHardwareCTRE("Swerve Gyro", gyro.getPigeon());
+    
+    return status;
   }
-
-  
-
 }