From b916df256d7757e7a6a996a2131ba776f961d116 Mon Sep 17 00:00:00 2001
From: C4llSiqn <zachwilke@icloud.com>
Date: Thu, 28 Nov 2024 16:55:08 -0700
Subject: [PATCH] add diagnosticStatus for SwerveModule, and add
 diagnoseHardwareCTRE inside Status

---
 .../robot/subsystems/SwerveModule.java        | 15 ++++++--
 src/main/java/frc4388/utility/Status.java     | 36 +++++++++++++++++++
 2 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 6607979..c5f99d6 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -16,12 +16,14 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.ControlRequest;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MagnetHealthValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.ctre.phoenix6.hardware.CANcoder;
@@ -251,10 +253,19 @@ public class SwerveModule extends Subsystem {
         return status;
     }
 
+    public boolean motorsAlive() {
+        return this.driveMotor.isAlive() && this.angleMotor.isAlive();
+    }
+
     @Override
     public Status diagnosticStatus() {
-        Log("Diagnostic info for this swereve module has not been inplemented!"); //TODO
-        return new Status();
+        Status status = new Status();
+        
+        status.diagnoseHardwareCTRE("Drive", this.driveMotor);
+        status.diagnoseHardwareCTRE("Angle", this.angleMotor);
+        status.diagnoseHardwareCTRE("Steer", this.encoder);
+        
+        return status;
     }
 
     // public double getCurrent() {
diff --git a/src/main/java/frc4388/utility/Status.java b/src/main/java/frc4388/utility/Status.java
index f4a0791..7f94266 100644
--- a/src/main/java/frc4388/utility/Status.java
+++ b/src/main/java/frc4388/utility/Status.java
@@ -3,6 +3,12 @@ package frc4388.utility;
 import java.util.ArrayList;
 import java.util.List;
 
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.controls.EmptyControl;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
+
 public class Status {
     public enum ReportLevel {
         INFO,
@@ -33,6 +39,36 @@ public class Status {
         this.reports.add(r);
     }
 
+    public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
+        if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
+        else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
+    }
+
+    public void diagnoseHardwareCTRE(String deviceName, CANcoder coder) {
+        // Because the Cancoder has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
+        // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
+        // TODO: validate that a CANCoder can actually do `EmptyControl`s
+        StatusCode status = coder.setControl(new EmptyControl()); 
+        if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Cancoder Alive?: Alive.");
+        else addReport(ReportLevel.ERROR, deviceName + " Cancoder Alive?: Dead!");
+        
+        // StatusSignal<MagnetHealthValue> -> MagnetHealthValue -> int
+        int coderMagHealth = coder.getMagnetHealth().getValue().value;
+        if (coderMagHealth == 3) addReport(ReportLevel.INFO, deviceName + " Cancoder Magnet Strength?: Ideal."); // why is 3 the 'good value'?
+        if (coderMagHealth == 2) addReport(ReportLevel.WARNING, deviceName + " Cancoder Magnet Strength?: Subpar.");
+        if (coderMagHealth == 1) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Too Close or Far!");
+        if (coderMagHealth == 0) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Unkown!");
+    }
+
+    public void diagnoseHardwareCTRE(String deviceName, Pigeon2 pigeon) {
+        // Because the Pigeon has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
+        // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
+        // TODO: validate that a Pigeon2 can actually do `EmptyControl`s
+        StatusCode status = pigeon.setControl(new EmptyControl()); 
+        if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Pigeon2 Alive?: Alive.");
+        else addReport(ReportLevel.ERROR, deviceName + " Pigeon2 Alive?: Dead!");
+    }
+    
     public boolean hasReport() {
         return reports.size() == 0;
     }