Align fix

[aidnem]

• only subtract pi radians when on red alliance and speaker, adds back pi radians for isDriveAligned method under same conditions