%% 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

%% ZERO CONTROL CASE
addpath('servofilter');         % Add path to servo
typeB1proto_no_control_150618;  % NO CONTROL
rmpath ('servofilter');         % Remove path to servo
mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
% SETTING FILTERS
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(-2,2,1001);
freq2=logspace(-1,1,2001);
freq0=logspace(-2,3,2001);
freq3=logspace(-2,1,2001);
freq4=logspace(-4,1,2001);

%% OL GAIN
dcdampflt = myzpk([1e-1;1e-1],[1e-4;1e1],10)...
          * myzpk([],[1e2;1e2],(1e2*pp)^2);
dcflt = myzpk([],[1e-4;1e1],100*1e-3*pp^2)...
          * myzpk([],[3e-1;3e-1],(3e-1*pp)^2);
      
clblstOL={...
    dcdampflt*gain_act_VF0,...
    dcflt*gain_act_VF1,...
    dcflt*gain_act_VF2,...
    };

doflst={...
     'actVF0','LVDT_VF0';
     'actVF1','LVDT_VF1';
     'actVF2','LVDT_VF2'};

bodesusplotcmpopt(sysc0,...
    doflst,freq4,...
    'title',...
    'Open Loop Gain of GAS filter controls with gain=1',...
    'color',{'r-','b-','g-'},...
    'calibration',clblstOL,'ylim',[1e-6,1e3]);
export_fig('figure/typeB1proto_GASdamp_OLTF_150703.pdf')





%% SETTING
addpath('servofilter');         % Add path to servo
typeB1proto_servo_lock_150629;  % LOCK ACQUISITION MODE
rmpath ('servofilter');         % Remove path to servo
gain_LIM=0;gain_TIM=0;gain_VIM=0;
gain_RIM=0;gain_PIM=0;gain_YIM=0;
gain_LTM=0;gain_PTM=0;gain_YTM=0;
gain_VF0=0;gain_VF1=0;gain_VF2=0;
dcdampflt = myzpk([1e-1;1e-1],[1e-4;1e1],10)...
          * myzpk([],[20;20],(20*pp)^2);
servo_VF0 = dcdampflt;
servo_VF1 = dcdampflt;
servo_VF2 = dcdampflt;

gainlist=[0,-0.1,-1,-10,-100];
sysd=cell(1,length(gainlist));
lgndlst=cell(1,length(gainlist));
clblst=cell(1,length(gainlist));





%% DAMPING ABOUT VF0
for i=1:length(gainlist)
gain_VF0=gainlist(i);
lgndlst{i}=['gain=',num2str(gainlist(i))];
clblst{i}=zpk([0,0],[],1);

mdlfile='typeB1simctrl_150616'; % typeB1 ver.150616
st     =linmod(mdlfile);
invl   =strrep(st.InputName, [mdlfile,'/'],'');
outvl  =strrep(st.OutputName,[mdlfile,'/'],'');
sysd{i}=ss(st.a,st.b,st.c,st.d,'inputname',invl,'outputname',outvl);
end
gain_VF0=gain_VF0*0;

%% PLOT ABOUT VF0
bodesusplotcmpopt2(sysd,'accVGND','VTM',freq2,...
    'ylim',[1e-6,1e2],'legend',lgndlst,'title',...
    'Transfer function from VGND to VTM with F0 controlled',...
    'ylabel','Magnitude [m/m]','calibration',clblst,...
    'xlim',[0.1,10]);
export_fig('figure/typeB1proto_GASdamp_VF0_150703.pdf')