%% ABOUT THIS FILE
% ----------------------------------------------------------------------
% Type-B1 prototype for KAGRA
% Coded by T. Sekiguchi on 2015/06/16
% ----------------------------------------------------------------------

%% PRELIMINARY
clear all;  % Clear workspace
close all;  % Close plot windows
addpath('../../utility');  % Add path to utilities
g = 9.81;

%% IMPORT SUSPENSION MODEL
matfile='typeBpsusmdl_woTF';
load([matfile,'.mat']);

%% TUNING DAMPER
% This part compensates the failure in converting the structural damping to
% viscous damping.
% REDUCE DAMPING ON RIM
sys1.a(22,22)=sys1.a(22,22)/30; % RIM
sys1.a(28,28)=sys1.a(28,28)/30; % RRM
sys1.a(34,34)=sys1.a(34,34)/30; % RTM
% INCREASE DAMPING ON YF0
%sys1.a(15,15)=sys1.a(15,15)*3000; % YF0
% INCREASE DAMPING ON YRM, YTM
sys1.a(30,30)=sys1.a(30,30)*30;   % YRM
sys1.a(36,36)=sys1.a(36,36)*300;  % YTM

%% IMPORT SERVO FILTERS
addpath('servofilter');         % Add path to servo
typeBptest_no_control_150722_type1;  % NO CONTROL MODE
rmpath ('servofilter');         % Remove path to servo

%% IMPORT SIMULINK MODEL
mdlfile='typeBpsimctrl_woTF_150804'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysc0  =ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);

%% FREQUENCY
freq1=logspace(-2,1,1001);
freq2=logspace(-2,2,1001);

%% TRANSFER FUNCTION F0
% data_LF0=importdata('measurement/20150529IP_L_tf.txt');
% data_LF0=[data_LF0(:,1),data_LF0(:,2),data_LF0(:,4)];
% data_TF0=importdata('measurement/20150529IP_T_tf.txt');
% data_TF0=[data_TF0(:,1),data_TF0(:,2),data_TF0(:,4)];
% data_YF0=importdata('measurement/20150529IP_Y_tf.txt');
% data_YF0=[data_YF0(:,1),data_YF0(:,2),data_YF0(:,4)];
% 
% bodesusplotmeascmp(sysc0,'actLF0','LVDT_LF0',freq1,data_LF0,...
%     'ylim',[1e-4,1e1],'calibration',gain_act_LF0,'unit1','1',...
%     'title','Transfer function from actLF0 to LF0');
% export_fig('figure/typeB1proto_TFcmp_LF0_150616.pdf')
% 
% bodesusplotmeascmp(sysc0,'actTF0','LVDT_TF0',freq1,data_TF0,...
%     'ylim',[1e-4,1e1],'calibration',gain_act_TF0,'unit1','1',...
%     'title','Transfer function from actTF0 to TF0');
% export_fig('figure/typeB1proto_TFcmp_TF0_150616.pdf')
% 
% bodesusplotmeascmp(sysc0,'actYF0','LVDT_YF0',freq1,data_YF0,...
%     'ylim',[1e-4,1e1],'calibration',gain_act_YF0,'unit1','1',...
%     'title','Transfer function from actYF0 to YF0');
% export_fig('figure/typeB1proto_TFcmp_YF0_150616.pdf')



%% TRANSFER FUNCTION IM

bodesusplotopt(sysc0,'accLGND','IFO_LTM',freq2);