Do the safety check after PID control