Motor Control Changes

Moved throttle calculation into one PID adjust calculation