From a6b43076f043963d7e90f34d97fe24bdbe5051b1 Mon Sep 17 00:00:00 2001
From: Technologyman00 <collinost@gmail.com>
Date: Mon, 20 Nov 2023 20:34:28 -0600
Subject: [PATCH] Display Errors with Reading the Absolute Encoders

Display Errors with Reading the Absolute Encoder
---
 swervelib/SwerveDrive.java  |  2 ++
 swervelib/SwerveModule.java | 18 ++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 043c71d..bd10794 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -901,6 +901,8 @@ public class SwerveDrive
               "Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition());
           SmartDashboard.putNumber(
               "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
+          SmartDashboard.putNumber(
+                "Module[" + module.configuration.name + "] Absolute Encoder Read Issue", module.getAbsoluteEncoderReadIssue());
         }
         if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
         {
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 28744bd..285b9b6 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -65,6 +65,11 @@ public class SwerveModule
    */
   private boolean                synchronizeEncoderQueued = false;
 
+  /**
+   * Absolute Encoder Read Issue Dectected. 
+   */
+  public boolean                  absoluteEncoderReadIssue = false;
+
   /**
    * Construct the swerve module and initialize the swerve module motors and absolute encoder.
    *
@@ -298,13 +303,16 @@ public class SwerveModule
     double angle;
     if (absoluteEncoder != null)
     {
+      absoluteEncoderReadIssue = false;
       angle = absoluteEncoder.getAbsolutePosition() - angleOffset;
       if (absoluteEncoder.readingError)
       {
+        absoluteEncoderReadIssue = true;
         angle = getRelativePosition();
       }
     } else
     {
+      absoluteEncoderReadIssue = true;
       angle = getRelativePosition();
     }
     angle %= 360;
@@ -387,4 +395,14 @@ public class SwerveModule
   {
     return configuration;
   }
+
+  /*
+   * Get if the last Absolute Encoder had a read issue.
+   * 
+   * @return If the last Absolute Encoder had a read issue.
+   */
+  public boolean getAbsoluteEncoderReadIssue()
+  {
+    return absoluteEncoderReadIssue;
+  }
 }