From e2cb296051dc5d797eeb4efeb256856c79b71699 Mon Sep 17 00:00:00 2001
From: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
Date: Tue, 21 Mar 2023 22:36:46 -0500
Subject: [PATCH] Moved telemetry to correctly report module angles

---
 swervelib/SwerveModule.java | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index eefada5..31ea182 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -156,15 +156,6 @@ public class SwerveModule
     desiredState =
         new SwerveModuleState2(
             simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond);
-    if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
-    {
-      SmartDashboard.putNumber(
-          "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
-      SmartDashboard.putNumber(
-          "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
-      SmartDashboard.putNumber(
-          "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
-    }
 
     if (isOpenLoop)
     {
@@ -195,6 +186,16 @@ public class SwerveModule
       angle += 360;
     }
 
+    if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+    {
+      SmartDashboard.putNumber(
+          "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
+      SmartDashboard.putNumber(
+          "Optimized " + moduleNumber + " Angle Setpoint: ", angle);
+      SmartDashboard.putNumber(
+          "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
+    }
+
     // Prevent module rotation if angle is the same as the previous angle.
     if (angle != lastAngle || synchronizeEncoderQueued)
     {