Auto-locking heading

[willitcode]

Allow robot heading to stay locked to the same angle using the gyro. I believe there's a method built into YAGSL for it.

[willitcode]

I thiiiiiink we basically just run SwerveDrive.setHeadingCorrection(true);???

[MarissaKoglesby]

Tried to fix the auto-heading-locking in the heading-locking branch. Implemented to branch but it caused the robot to move weirdly and crazily. @willitcode

[willitcode]

Heard. Will look into further.