Add PID controller

[DavidFair]

Add a new PID controller which takes a target speed, current speed and sets the target PWM values

• [x] Add PID controller class with stub functions
• [x] Add to main loop
• [x] Add support for calculating speed in encoder + unit tests
• [x] Implement stubs in PID controller for each step
• [x] Add setting a speed via serial

[DavidFair]

Completed but forgot to close