%周波数応答計算用プログラム
%FFTを使って周波数応答を実測から計算する
%2005.08.22 T.Ogawa 作成 Octave用

clear
format long e

%データファイルに関して
data_num_min=1; %データファイル数(最小)
data_num_max=5;%データファイル数(最大)
msec2sec=1000;     %時間が[msec]なら'1000'を[sec]なら'1'を代入．

%データ列に関して
col_u=2;            %入力 (分母)列の指定
col_y=3;            %出力 (分子)列の指定　
gain_u=1;           %入力のゲイン設定
gain_y=1;           %出力のゲイン設定


%フラッグ
flag_base=3;        %最大をとる基準値の変更.'0':入力列（分母）. '1':出力列（分子）
                    %                       '2':入力列（分母）. '3':出力列（分子）
                    %                       '4':範囲指定　入力列（分母）. '5':範囲指定出力列（分子）
f_range = 100;          %　　　　　　　　　　　 '4'，'5'を選択したとき最大周波数のf_range[%]内で最大を探索．
                    %注.ノイズに強いデータを基準値にするほうがよい.
flag_sampling=1;    %サンプリング周期の自動指定. '1':自動．'0':手動

flag_gain=0;        %デシベル表示なら'0',RATIO表示なら'その他'

flag_sin=1;         %0ならSin波表示．1ならSin波表示省略．

%その他
k=1;        		%カウンター

for data_num = data_num_min:data_num_max

%読み込みデータファイル名の指定

	filename00='D:\\05_03\\98Test\\05_08\\05_08_11ASS500_050R_SN002_Sin2\\sin_cal\\';
	filename01='050811_Sin';
	filename02=num2str(data_num);
	filename03='.CSV';
	loadfile=strcat(filename00,filename01,filename02,filename03)
	loadfile_short = strcat(filename01,filename02,filename03);

%データのロード
    		data = dlmread(loadfile,',');

%サンプリング周期[sec]の設定
	if (flag_sampling==1)
    		line01=3;
    		line02=103;
    		ij=1;
    		for ii=line01:line02
        		err(ij,1)=data(ii+1,1)-data(ii,1);
        		ij=ij+1;
    		end
    		TT = sum(err)/length(data(line01:line02,1))/msec2sec;
	elseif(flag_sampling==0)
   		TT = 100e-6;           %サンプリング周期[sec]
	end
	F=1/TT;     		         %サンプリング周波数[Hz]


%データサイズの指定
	[Row,Col]=size(data);


%入力(分母)のFFT
	y01=fft(data(:,col_u)*gain_u);
	MAG01=abs(y01)/(Row/2);
	PHASE01=unwrap(angle(y01));

%出力(分子)のFFT
	y02=fft(data(:,col_y)*gain_y);
	MAG02=abs(y02)/(Row/2);
	PHASE02=unwrap(angle(y02));

%周波数
	f = (0:length(y01)-1)'*F/length(y01);

	if(flag_sin == 0)

	subplot(2,1,1)
	clearplot();
	subplot(2,1,2)
	clearplot();

	subplot(2,1,1)
	ylabel('Amplitude')
	xlabel('freqency[Hz]')
	title(loadfile_short)
	semilogx(f(2:round(Row/2)),MAG01(2:round(Row/2)),'-;;',f(2:round(Row/2)),MAG02(2:round(Row/2)),'-;;')


	subplot(2,1,2)
	ylabel('Amplitude')
	xlabel('time')

	plot(data(:,1),data(:,col_u)*gain_u,'r',data(:,1),data(:,col_y)*gain_y)

	end

%	max(data(:,col_u)*gain_u)
%	max(data(:,col_y)*gain_y)

%	disp('__')
%	disp('Prease Push Enter Key')

	if (flag_base==0)
    		[ref_gain_max,max_row] = max(MAG01(2:Row));
    		act_gain_max = MAG02(max_row+1);
	elseif (flag_base==1)
    		[act_gain_max,max_row] = max(MAG02(2:Row));
    		ref_gain_max = MAG01(max_row+1);
	elseif (flag_base==2)
%    		[ref_gain_max,max_row] = max(MAG02(2:round(Row^0.5)));
    		[ref_gain_max,max_row] = max(MAG01(2:round(Row/2)));
    		act_gain_max = MAG02(max_row+1);
	elseif (flag_base==3)
%    		[act_gain_max,max_row] = max(MAG01(2:round(Row^0.5)));
    		[act_gain_max,max_row] = max(MAG02(2:round(Row/2)));
     		ref_gain_max = MAG01(max_row+1);
	elseif (flag_base==4)
%    		[ref_gain_max,max_row] = max(MAG02(2:round(Row^0.5)));
    		[ref_gain_max,max_row] = max(MAG01(2:round((Row/2)*f_range/100)));
    		act_gain_max = MAG02(max_row+1);
	elseif (flag_base==5)
%    		[act_gain_max,max_row] = max(MAG01(2:round(Row^0.5)));
    		[act_gain_max,max_row] = max(MAG02(2:round((Row/2)*f_range/100)));
    		ref_gain_max = MAG01(max_row+1);
	end
    
    
%GAIN,PHASEの計算
	servo_frq(k) = f(max_row+1);
	servo_phase(k) = -(PHASE01(max_row+1) - PHASE02(max_row+1))*360/pi/2;
	if (flag_gain==0)
    		servo_gain(k) = 20*log10(act_gain_max/ref_gain_max);
	elseif(flag_gain==1)
    		servo_gain(k) = act_gain_max/ref_gain_max;
	end


%PHASEの調整(-360度～0度内に収める)
	for count=0:100000
		if (servo_phase(k)>0)
    			servo_phase(k)=servo_phase(k)-360;
		end
		if (servo_phase(k)<-360)
        		servo_phase(k)=servo_phase(k)+360;
		end
	end    

	k=k+1;
end


%計算結果の出力
%servo_frq'
%servo_gain'
%servo_phase'
f_response=[servo_frq',servo_gain',servo_phase'];



%figure(1)
subplot(2,1,1)
clearplot();
subplot(2,1,2)
clearplot();

gset('lmargin 13')

subplot(2,1,1)
xlabel('frequency [Hz]')
ylabel('Gain [dB]')

%axis([1 100 -120 20],'auto');
axis([1 100 -120 20]);
grid('on')

semilogx(f_response(:,1),f_response(:,2),'o;;')

subplot(2,1,2)
xlabel('freqency[Hz]')
ylabel('Phase [deg]')
%axis([1 100 -200 0],'auto');
axis([1 100 -200 200]);
grid('on')
semilogx(f_response(:,1),f_response(:,3),'o;;')

%計算結果のファイルへの出力
%save -ascii result.txt f_response
%dlmwrite('D:\\result.csv',f_response,',');

