WPILIB Trajectory Following-our robot is either very jerky or doesn't follow path

Hello, I am having trouble with the WPILIB trajectory tutorial that I followed along with step-by-step. I completed the characterization and entered the constants into my program, though the characterization data could have been a little better. The robot correctly follows a simple straight line path, however, doing anything else causes problems.
Code:

I followed along with the trajectory troubleshooting and nothing seemed to help.
Following the trajectory I have in my code currently, here is what happens.
Note: the times in the translation graphs and the others are not necessarily synced. And the graphs have similar shapes, but notice that the units/scale on the side are different. I believe in theory, they should have similar shapes and similar scales(output ranges).

If I have the ramsete controller enabled and the PID controller enabled, this is the result (I believe this one followed the trajectory correctly, though jerky):

If I have the ramsete controller enabled and the PID controller disabled, this is the result (I believe this one followed the trajectory wrong and was jerky):

If I have the ramsete controller disabled and the PID controller enabled, this is the result (Instead of following path, robot moved forward a little, turned to the right 45ish degrees, then moved forward more):

If I have the ramsete controller disabled and the PID controller disabled, this is the result (robot moved the same way as the last one):

Here is a video that demonstrates the jerky movement. It follows the trajectory that I told it to. I would obviously like to be able to go faster than this consistently. There have been times the robot has correctly followed the trajectory at a higher speed than this, but I don’t have a video of it. However, the jerky-ness in that scenario looked similar to this video.
https://drive.google.com/file/d/1LuQ08xHcxq6CfuEwHgrhNyLj8SLMMQHr/view?usp=sharing

Here is a video that demonstrates the robot following the wrong path.
https://drive.google.com/file/d/1PVwzZT37uMlk50rs7LiL24SqPM26t3EP/view?usp=sharing

The only difference in the code between these videos was in the “kMaxSpeedMetersPerSecond” value in the Constants class. In the first video, it was 0.5. In the second video, it was either 1 or 3 (cant remember).
Also, idk if this is related but I am also getting errors that are telling me my TalonSRX drive motors are not being updated enough, however teleop still works fine. Thanks!

Edit: changing the drive motor mode to Coast instead of Brake does not change much. And I got the robot to not turn around at the start or finish turned around at the end by reversing the trajectory config and setting the final pose to have a rotation of 180 degrees (pi radians), though the initial pose is still 0 degrees.

Edit: problem resolved. I forgot to negate the getGyroRotation2d() method. Robot runs smoothly now. My bad

3 Likes