Added reverse direction to PID controller (choose direction based on current error)