Merge pull request #41 from frc937/heading-snapping-bug

Heading snapping bug

src/main/java/frc/robot/commands/drive/DriveFieldOrientedHeadingSnapping.java

@@ -37,7 +37,9 @@ public DriveFieldOrientedHeadingSnapping() {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    drive.setHeadingCorrection(true);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -78,7 +80,9 @@ public void execute() {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drive.setHeadingCorrection(false);
+  }
 
   // Returns true when the command should end.
   @Override

src/main/java/frc/robot/subsystems/Drive.java

@@ -77,6 +77,16 @@ public void enterXMode() {
     drive.lockPose();
   }
 
+  /**
+   * Set the heading correction capabilities of YAGSL. Should only be enabled when heading
+   * correction capabilities are in use
+   *
+   * @param state SwerveDrive.headingCorrection state
+   */
+  public void setHeadingCorrection(boolean state) {
+    drive.setHeadingCorrection(state);
+  }
+
   /**
    * Takes [-1, 1] joystick-like inputs and converts them to a {@link ChassisSpeeds} object that
    * represents the commanded robot velocities