Normalize the goal vector, cap the angular velocity, and add better debug prints.