Added simple PID control loop

This PID loop simply tries to reach a certain speed, with no regard for the
total distance traveled.

Signed-off-by: Morten Hustveit <morten.hustveit@gmail.com>