%Frequency response of an AFM system
% kja 070320

aminit;

% Data for the AFM
afm_data;

sys=ss(A,B,C,D);
num=w3^2*[m2 c k];
den=conv([m1+m2 c k],[1 2*z3*w3 w3^2]);
w=logspace(4,7,2000);
%w=logspace(1,3,2000);
[num2,den2]=ss2tf(A,B,C,0);
[mag,phase]=bode(num,den,w);
[mag2,phase2]=bode(num2,den2,w);

% Store the locations of the peaks and valleys (manually determined)
omega1 = 2.478e5;
omega2 = 2.684e5;
omega3 = 7.9e5;

figure(1); clf; subplot(221);
[magh, phaseh] = ambode(sys);

% Annotate the gain
subplot(magh); hold on;
amaxis([1e4 1e7 1e-2 1e2]);
loglog([omega1 omega1], [1e-2 1.5], 'k--');
amtext(0.75*omega1, 8, 'Mr1');
amtext(0.35*omega1, 0.05, 'w=wr1');
% loglog([omega2 omega2], [1e-2 1e2], 'k--');
loglog([omega3 omega3], [1e-2 3.5], 'k--');
amtext(0.75*omega3, 15, 'Mr2');
amtext(1.1*omega3, 0.05, 'w=wr2');

% Annotate the phase
subplot(phaseh); hold on;
amaxis([1e4 1e7 -185 10]);
semilogx([omega1 omega1], [-185 5], 'k--');
% semilogx([omega2 omega2], [-185 5], 'k--');
semilogx([omega3 omega3], [-185 5], 'k--');

amprint('afm-freqresp.eps');
return;

% Old code
pl=loglog(w,mag,'b',w,mag2,'r--');
set(pl,'Linewidth', AM_data_linewidth);
axis([1e4 1e7 0.01 10])
ylabel('Gain');
subplot(323)
pl=semilogx(w,phase,'b',w,phase2,'r--');
set(gca,'Ytick',[-180 -90 0]);
set(pl,'Linewidth', AM_data_linewidth);
axis([1e4 1e7 -180 5]);
xlabel('w');
ylabel('Phase');

