diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index d31db0d5..f1d35a02 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -98,10 +98,6 @@ public class SwerveDrive
    * Deadband for speeds in heading correction.
    */
   private       double                   HEADING_CORRECTION_DEADBAND                     = 0.01;
-  /**
-   * Whether heading correction PID is currently active.
-   */
-  private       boolean                  correctionEnabled                               = false;
   /**
    * Swerve IMU device for sensing the heading of the robot.
    */
@@ -453,16 +449,12 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
           && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
               || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
       {
-        if (!correctionEnabled)
-        {
-          lastHeadingRadians = getOdometryHeading().getRadians();
-          correctionEnabled = true;
-        }
         velocity.omegaRadiansPerSecond =
-            swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
-      } else
+            swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians);
+      }
+      else
       {
-        correctionEnabled = false;
+        lastHeadingRadians = getOdometryHeading().getRadians();
       }
     }