Ramsete trajectory following turning issues

Hey there, our team is having issues with trajectory tracking via ramsete specifically with turning paths. When I try to implement a straight path into the ramsete command, the robot can go relatively straight but sometimes at the begining of the path the robot turns 80-90 degree clockwise. However I noticed if i add the following code:

if (DriveTrainSubsystem.getHeading() != 0) {
DriveTrainSubsystem.zeroHeading();
}

in the autonomous periodic function in robot.java, the robot goes straight perfectly fine. This quick fix/cheat works, but is pretty useless because it only worked for a path that is a 180 degree perfectly straight line.

The real problem occurs when I try to implement paths with turns. The robot initially turns 90 degrees clockwise (even though it is supposed to just follow a straight path) and somewhat follows the path but when it comes time to turn the robot jerks back and forth in place trying to move and essentially looks like it’s fighting with itself.

Videos. Check title of each video to see the problem

I also included our code in the google drive link.

Any help is appreciated!

make sure your gyro is in phase with your drivetrain, that is your gyros understanding of a “positive” direction is the same as your motors, an easy way to test this is when you update your pose negate your gyro and see if the behavior improves.