Add a proportional controller for motor speed

Adjust wait time based on historical error, this makes the controller's
speed much more accurate.