[Feature] PID

[Starflight-code]

Is your feature request related to a problem? Please describe. A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]

Add/adapt PID control for drive motors

Describe the solution you'd like A clear and concise description of what you want to happen.

Adapt the PID algorithm, update constants and integrate the functionality into autonomous drive systems.

Describe alternatives you've considered A clear and concise description of any alternative solutions or features you've considered. N/A

Additional context Add any other context or screenshots about the feature request here. N/A

[Starflight-code]

New PID code has been pushed to the testing branch, pending implementation and testing. This will be evaluated at a later date once we can test the code and verify its accuracy. Tuning the static variables will also need to be completed, likely by trial and error.

[Starflight-code]

Tested and working, static variables should be tuned, but it works well enough for now.