From 8f0ed77b6ca8cda3abf772bc6148522d13c4d718 Mon Sep 17 00:00:00 2001
From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com>
Date: Fri, 26 Jan 2024 11:26:21 -0600
Subject: [PATCH] Added SwerveDrive.getGyro(). reformatted code.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
---
 src/main/java/swervelib/SwerveDrive.java      | 48 +++++++++++--------
 src/main/java/swervelib/SwerveModule.java     |  3 +-
 .../parser/SwerveModuleConfiguration.java     |  2 +-
 .../swervelib/parser/json/ModuleJson.java     |  2 +-
 4 files changed, 32 insertions(+), 23 deletions(-)

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index f1d35a02..96e165b6 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -451,8 +451,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
       {
         velocity.omegaRadiansPerSecond =
             swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians);
-      }
-      else
+      } else
       {
         lastHeadingRadians = getOdometryHeading().getRadians();
       }
@@ -679,6 +678,33 @@ public SwerveModulePosition[] getModulePositions()
     return positions;
   }
 
+  /**
+   * Getter for the {@link SwerveIMU}.
+   *
+   * @return generated {@link SwerveIMU}
+   */
+  public SwerveIMU getGyro()
+  {
+    return swerveDriveConfiguration.imu;
+  }
+
+  /**
+   * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
+   * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
+   *
+   * @param gyro expected gyroscope angle as {@link Rotation3d}.
+   */
+  public void setGyro(Rotation3d gyro)
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
+    } else
+    {
+      setGyroOffset(imu.getRawRotation3d().minus(gyro));
+    }
+  }
+
   /**
    * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
    */
@@ -1042,24 +1068,6 @@ public void addVisionMeasurement(Pose2d robotPose, double timestamp)
 //    resetOdometry(newOdometry);
   }
 
-
-  /**
-   * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
-   * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
-   *
-   * @param gyro expected gyroscope angle as {@link Rotation3d}.
-   */
-  public void setGyro(Rotation3d gyro)
-  {
-    if (SwerveDriveTelemetry.isSimulation)
-    {
-      setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
-    } else
-    {
-      setGyroOffset(imu.getRawRotation3d().minus(gyro));
-    }
-  }
-
   /**
    * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to
    * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply
diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java
index bcc3eb6a..260cdf06 100644
--- a/src/main/java/swervelib/SwerveModule.java
+++ b/src/main/java/swervelib/SwerveModule.java
@@ -205,7 +205,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
     } else
     {
       double cosineScalar = 1.0;
-      if (configuration.useCosineCompensator) {
+      if (configuration.useCosineCompensator)
+      {
         // Taken from the CTRE SwerveModule class.
         // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
         /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
index 0fbe0553..24d06257 100644
--- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
+++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
@@ -164,7 +164,7 @@ public SwerveModuleConfiguration(
         false,
         false,
         false,
-        name, 
+        name,
         useCosineCompensator);
   }
 
diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java
index 9b9e6aae..b9f24043 100644
--- a/src/main/java/swervelib/parser/json/ModuleJson.java
+++ b/src/main/java/swervelib/parser/json/ModuleJson.java
@@ -56,7 +56,7 @@ public class ModuleJson
   /**
    * Should do cosine compensation when not pointing correct direction;.
    */
-  public boolean           useCosineCompensator = true;
+  public boolean           useCosineCompensator    = true;
 
   /**
    * Create the swerve module configuration based off of parsed data.