Add PID based servo control. It needs some tuning work as it's
overshooting a lot and bouncing, but I think that may be resolved with
the intelligent use of motor braking. Need to think about how to
integrate that feature.