Turn to Angle Swerve Drive

So our team was experimenting with trying to turn to an angle using swerve drive but our team was finding no success after many trials. For reference our code to turn to an angle can be seen here: 5549-2023/AutoAlign2Z.java at main · FRC5549Robotics/5549-2023 · GitHub.

Does anyone have an idea as to why the code isn’t working or does anybody have example code as to how to turn the robot to a specific angle using swerve drive?

Gryphon Robotics.