Fix drive distance PID

[kreeve]

Taha and I identified a few possible bugs during AZ. We should try to fix those so we don't have to rely on a timeout.

[ghost]

Yes, I believe we posted them on slack. There seemed to be minor flaws in our logic, but mostly one line fixes?

[camilo86]

I'm not entirely sure if this was the error, but the PID enable() was commented out along with setInputRange() & setOutputRange() I added it back in here 4bbaffc