%% 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='typeB1susmdl';
load([matfile,'.mat']);

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

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

%% IMPORT SIMULINK MODEL
mdlfile='typeB1simctrl_150616'; % 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
freq =logspace(-4,2,1001);
freq2=logspace(-2,3,1001);

%% TRANSFER FUNCTION F0
%bodesusplotopt(sysc0,'actLF0','LVDT_LF0',freq,...
%    'ylim',[1e-4,1e1],'calibration',gain_act_LF0,'unit1','1',...
%    'position',[0 50 800 650]);
%export_fig('figure/typeB1proto_TF_LF0_150616.pdf')

bodesusplotcmpopt(sysc0,...
    {'actLF0','LVDT_LF0';'actTF0','LVDT_TF0';'actYF0','LVDT_YF0'},freq,...
    'ylim',[1e-3,1e3],'calibration',...
    {gain_act_LF0*servo_LF0,gain_act_TF0*servo_TF0,gain_act_YF0*servo_YF0},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'LF0','TF0','YF0'});
export_fig('figure/typeB1proto_OLTF_F0_150617.pdf')



%% TRANSFER FUNCTION IM
bodesusplotcmpopt(sysc0,...
    {'actLIM','OSEM_LIM';'actTIM','OSEM_TIM';'actVIM','OSEM_VIM'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_LIM*servo_LIM,gain_act_TIM*servo_TIM,gain_act_VIM*servo_VIM},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'LIM','TIM','VIM'});
export_fig('figure/typeB1proto_OLTF_IM1_150617.pdf')

bodesusplotcmpopt(sysc0,...
    {'actRIM','OSEM_RIM';'actPIM','OSEM_PIM';'actYIM','OSEM_YIM'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_RIM*servo_RIM,gain_act_PIM*servo_PIM,gain_act_YIM*servo_YIM},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'RIM','PIM','YIM'});
export_fig('figure/typeB1proto_OLTF_IM2_150617.pdf')



%% TRANSFER FUNCTION TM
bodesusplotcmpopt(sysc0,...
    {'actLTM','OSEM_LTM';'actPTM','OSEM_PTM';'actYTM','OSEM_YTM'},freq2,...
    'ylim',[1e-5,1e2],'calibration',...
    {gain_act_LTM*servo_LTM,gain_act_PTM*servo_PTM,gain_act_YTM*servo_YTM},...
    'unit1','1','title','Open Loop Gain',...
    'legend',{'LTM','PTM','YTM'});
export_fig('figure/typeB1proto_OLTF_TM_150617.pdf')



%% TRANSFER FUNCTION OpLev
bodesusplotcmpopt(sysc0,...
    {'actPIM','OpLev_PTM';'actPTM','OpLev_PTM'},freq2,...
    'ylim',[1e-7,1e2],'calibration',...
    {gain_act_PIM,gain_act_PTM},...
    'unit1','1','title','Open Loop Gain',...
    'position',[0 50 800 650]);
export_fig('figure/typeB1proto_TF_OpLev_PTM_150616.pdf')

bodesusplotcmpopt(sysc0,...
    {'actYIM','OpLev_YTM';'actYTM','OpLev_YTM'},freq2,...
    'ylim',[1e-7,1e2],'calibration',...
    {gain_act_YIM,gain_act_YTM},...
    'unit1','1',...
    'position',[0 50 800 650]);
export_fig('figure/typeB1proto_TF_OpLev_YTM_150616.pdf')

%% TRANSFER FUNCTION IFO
bodesusplotcmpopt(sysc0,...
    {'actLF0','IFO_LTM';'actLIM','IFO_LTM';'actLTM','IFO_LTM'},freq2,...
    'ylim',[1e-7,1e2],'calibration',...
    {gain_act_LF0,gain_act_LIM,gain_act_LTM},...
    'unit1','1',...
    'position',[0 50 800 650]);
export_fig('figure/typeB1proto_TF_IFO_150616.pdf')