@Anomaly, @theol0403 it is great that you have shared your code in an easy to read and copy&paste format, along with the clear to understand explanations!
This is what matters for the many beginner teams, for whom going to a github repository and extracting relevant code from a tangled codebase of another team might be too much of a challenge.
Since you asked about heading correction code, here is a relevant thread with a sample code:
@theol0403 one thing I noticed in your code is that you don’t check if the “dT” is 0 before dividing by it. You may want to do something about it, like not calculating derivative during the first step, because it could generate exception.
Another potential issue with using derivative is the measurement noise because if dT is a small number you can get roundoff errors and jumpy “D” component. A better solution may be to use V5 builtin get_actual_velocity() function. (RobotMesh reference)