% This file calculates frequency responses of the LSC lm
% servo. It assumes that 'mich' has been run, such that the 
% state-space model of the servo is in the variable 'lmsys'
makeDARMsys;
s = tf('s');
% set up frequencies
fmin = 1;
fmax = 1e4;
f = logspace(log10(fmin), log10(fmax), 1e3);

% open loop gain
clg = darmsys(4,7);
olg = -darmsys(2,7) / clg;

% Actuation Function (from DARM_OUT to mirror motion)
ActFunc = darmsys(5,7) / darmsys(3,7);

% Sensing Function (from GW in to DARM_IN1)
SensFunc = darmsys(2,1) / darmsys(1,1);


olgxz = squeeze(freqresp(olg, 2*pi*f));

UGF = 150;   % desired UGF
[m,p] = bode(olg, 2*pi*UGF);


%%Luke Burks 7/25/13
poleFreq = 393; %[Hz]
cavityResponse = ss(zpk([], -2*pi*poleFreq, 2*pi*poleFreq));
zeroFreq = 7000;
invSens = ss(zpk([-2*pi*poleFreq],[-2*pi*zeroFreq -2*pi*10000],2*pi*poleFreq));




huvt1 = darmsys(2,1) / SensFunc;
huvt2 = darmsys(3,1) * ActFunc;

huvt = huvt1 - huvt2;

%figure(77)
%margin(olg)
%xlim([1 7e3])

%% Make the plots
if 1==0
    
figure(87090)

top = subplot('Position',[0.13,0.52,0.82,0.4]);
semilogx(f, 20*log10(abs(olgxz)), 'b');
grid
set(top,'XTickLabel',[]);
axis tight
set(gca,'YTick',[-120:20:140])
ylim([-50 100])
ylabel('Magnitude [dB]')
title(' DARM Open Loop Gain')


bottom = subplot('Position',[0.13,0.1,0.82,0.4]);
semilogx(f, 180/pi*(angle(olgxz)), 'b');
grid
axis tight
set(gca,'YTick',[-360:45:360])
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')

  
figure(77112)
bode(huvt1, huvt2, huvt, 2*pi*f)
legend('Control','Error','Strain')

end

