%周波数応答計算用プログラム MATLAB用　2006/06/16　
%FFTを使って周波数応答を実測から計算する

%使用するデータに関する注意
%1.よく第1行,及び第2行は文字列となってあるが,読み込めないので注意のこと.
%2.サーボ弁の周波数応答は高い応答であるのでデータ数は多いほうがよい
%  EX.サンプリング0.2[msec],計測点数25000点.

% 2012/11/12 flag_sampling = 1　のときの処理内のプログラムミスを削除．

clear
format long e

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

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


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

freq_set = [ 2.0
             3.16
             5.01
             7.94
             10.0
             12.6
             20.0
             31.6 ];    %[Hz]　flag_baseにて6を設定した時，freq_set*(100 ±f_range )[%]以内で探索．

flag_sampling=1;    %サンプリング周期の自動指定. '1':自動．'0':手動
flag_gain=0;        %デシベル表示なら'0',RATIO表示なら'その他'

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

FFT_Gain = [];

for data_num=data_num_min:data_num_max

%読み込みデータファイル名の指定
filename00='D:\06kadai_01\98Test\06_06\06_06_16アクティブダンサ試験装置加振変位から荷重F特\';
filename01='060615_04g加振xActiveF_0';

%上記の例の場合　読み込みデータファイル名は
%
% D:\06kadai_01\98Test\06_06\06_06_16アクティブダンサ試験装置加振変位から荷重F特\060615_04g加振xActiveF_01.csv
% D:\06kadai_01\98Test\06_06\06_06_16アクティブダンサ試験装置加振変位から荷重F特\060615_04g加振xActiveF_02.csv
%													:
%													:
% D:\06kadai_01\98Test\06_06\06_06_16アクティブダンサ試験装置加振変位から荷重F特\060615_04g加振xActiveF_10.csv
% D:\06kadai_01\98Test\06_06\06_06_16アクティブダンサ試験装置加振変位から荷重F特\060615_04g加振xActiveF_11.csv
%													:
%													:

if data_num >= 10
    filename02=num2str(data_num);
else
    filename02 = strcat('0',num2str(data_num));    
end
    filename03='.csv';

    loadfile=strcat(filename00,filename01,filename02,filename03)

%データのロード
    data=load (loadfile);

%サンプリング周期[sec]の設定
if (flag_sampling==1)
    line01=3;
    line02=103;
    for ii=line01:line02
        err(i)=data(ii+1,1)-data(ii,1);
    end
    T=sum(err)/length(data(line01:line02,1))/msec2sec;
elseif(flag_sampling==0)
   T=100e-6;           %サンプリング周期[sec]
end
F=1/T;     		         %サンプリング周波数[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);

figure(1)
subplot(2,1,1)
%semilogx(f,MAG01,'r',f,MAG02,'b')

% semilogx(f(2:round(Row/2)),MAG01(2:round(Row/2)),'r',f(2:round(Row/2)),MAG02(2:round(Row/2)),'b')

%semilogx(f(2:round((Row/2)*f_range/100)),MAG01(2:round((Row/2)*f_range/100)),'r',f(2:round((Row/2)*f_range/100)),MAG02(2:round((Row/2)*f_range/100)),'b')
%semilogx(f(2:round((Row/2)*f_range/100)),PHASE02(2:round((Row/2)*f_range/100))-PHASE01(2:round((Row/2)*f_range/100)),'r')


%legend('Input','Output')

%ylabel('Amplitude')
%xlabel('freqency[Hz]')
%title(loadfile)
%subplot(2,1,2)
%plot(data(:,1),data(:,col_u)*gain_u,'r',data(:,1),data(:,col_y)*gain_y)
%ylabel('Amplitude')
%xlabel('time')

%legend('Input','Output')
max(data(:,col_u)*gain_u)
max(data(:,col_y)*gain_y)

%disp('　')
%disp('Enter')
%input('\n');

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(MAG01(2:round(Row/2)));
    act_gain_max = MAG02(max_row+1);
elseif (flag_base==3)
    [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(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(MAG02(2:round((Row/2)*f_range/100)));
    ref_gain_max = MAG01(max_row+1);
elseif (flag_base==6)

    fmin = f(Row/2);
    fmax = f(1);
    
    for kk = 1:(Row/2)

     if(f(kk)>=freq_set(k)*(1-f_range/100))
        if(f(kk)<fmin)
            fmin = f(kk);
            fmin_col = kk;
        end
     end
     
    if(f(kk)<=freq_set(k)*(1+f_range/100))
        if(f(kk)>fmax)
            fmax = f(kk);
            fmax_col = kk;
        end
     end
     
    end
    
    [ref_gain_max,max_row] = max(MAG01(fmin_col:fmax_col));
    max_row = max_row+fmin_col-2;
    act_gain_max = MAG02(max_row+1);
end
    
    
%GAIN,PHASEの計算
servo_frq(k) = f(max_row+1);
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
FFT_Gain = [FFT_Gain
            ref_gain_max act_gain_max];

%PHASEの調整(-180度～180度内に収める)

real_k = real(y02(max_row+1))*real(y01(max_row+1))+imag(y02(max_row+1))*imag(y01(max_row+1));
imag_k = real(y01(max_row+1))*imag(y02(max_row+1))-real(y02(max_row+1))*imag(y01(max_row+1));

servo_phase(k) = angle( complex(real_k,imag_k ))*180/pi;

k=k+1;
end


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


figure(1)
subplot(2,1,1)
semilogx(f_response(:,1),f_response(:,2),'.')
xlabel('freqency[Hz]')
ylabel('gain　[db]')
axis([1 200 -100 100])
grid on

subplot(2,1,2)
semilogx(f_response(:,1),f_response(:,3),'.')
xlabel('freqency[Hz]')
ylabel('phase[deg]')
axis([1 200 -180 180])
grid on

break
%計算結果のファイルへの出力
%save result.txt f_response -ASCII
csvwrite('060613_02frq_test.csv',f_response)