Forgot scaling factor for p_gain etc.