Merge pull request #150 from Blargleflakes/YAGSL_issue_44

Heading correction logic now reads in current heading
BroncBotz3481 · Jan 26, 2024 · 7089f41 · 7089f41
2 parents 37e9c5e + 23fca48
commit 7089f41
Showing 1 changed file with 4 additions and 12 deletions.
diff --git 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();
       }
     }