Untested PID speed controller code

First revision code - untested