Alistair's beginning of PI control to ensure greater accuracy and allow time for the encoder values to be read.

Signed-off-by: JHolland <jonathan.holland8@gmail.com>