%Loop shaping desing for an AFM in tapping mode
% kja 070705
aminit					% MATLAB initialization

% Parameter definitions normalized for with the frequency z*w0
% renormalized in plot
z=0.002;                % Relative damping 
n=20;                   % Number of periods for averaging
Tn=2*pi*n*z; 
N=10000;
%
%   Transfer function
w=logspace(-2,2,N);
one=ones(size(w));
s=i*w;
P=(1-exp(-s*Tn))./(s.*Tn.*(s+1));
k=3.5;
ki=8.3;
C=k+ki./s;;
L=C.*P;
Li=ki*P./s;
%   Phase crossover
[xxx,iii]=min(abs(angle(Li(1:7000)+pi)));wgc=w(iii);gm=1/abs(Li(iii));

%   Bode plot
figure(1); clf;
subplot(331);
loglog(w, abs(P), '--', w, abs(L), '-', w, abs(Li), '-.', ...
  'LineWidth', AM_bode_linewidth);
hold on;
loglog(w, one, 'k', 'LineWidth', AM_ref_linewidth);
loglog(wgc, 1, 'ro', 'MarkerSize', AM_bode_markersize);
set(gca, 'Ytick', [0.01 1 100])
amaxis([0.01 100 0.001 100]);
ylabel('g');

lgh = legend('P(s)', 'PI', 'Integral', 'Location', 'southwest');
legend(lgh, 'boxoff');

subplot(334);
Pp=180*unwrap(angle(P))/pi;
Lp=180*unwrap(angle(L))/pi;
Lip=180*unwrap(angle(Li))/pi;
semilogx(w, Pp, '--', w, Lp, '-', w, Lip, '-.','Linewidth', AM_bode_linewidth);
hold on;
semilogx(wgc, -180, 'ro', 'MarkerSize', AM_bode_markersize);
set(gca,'Ytick',[-270 -180 -90 0])
axis([0.01 100 -270 0]);
xlabel('w');ylabel('p')
semilogx(w,-180*one);
amprint('afm-ltfbode.eps');

figure(2); clf
%   Gang of Four
S=1./(1+L);
T=L.*S;
PS=P.*S;
CS=C.*S;
[Ms,iii]=max(abs(S));
[Mt,iii]=max(abs(T));
%
%   Plotting
subplot(332);
loglog(w,one, 'k', 'LineWidth', AM_ref_linewidth); hold on;
pl=loglog(w,abs(T),'b-');hold on;
set(pl,'Linewidth',AM_bode_linewidth)
axis([0.01 100 0.1 5]);
ylabel('T');

subplot(333);
loglog(w,one, 'k', 'LineWidth', AM_ref_linewidth); hold on;
pl=loglog(w,abs(PS),'b-');hold on;
set(gca,'Ytick',[0.01 0.1 1])
set(pl,'Linewidth',AM_bode_linewidth)
axis([0.01 100 0.01 2]);
ylabel('PS');

subplot(335);
loglog(w,one, 'k', 'LineWidth', AM_ref_linewidth); hold on;
pl=loglog(w,abs(CS),'b-');hold on;
set(pl,'Linewidth',AM_bode_linewidth)
axis([0.01 100 0.5 10]);
ylabel('CS');xlabel('w');

subplot(336);
loglog(w, one, 'k', 'LineWidth', AM_ref_linewidth); hold on;
pl=loglog(w,abs(S),'b-');hold on;
set(pl,'Linewidth',AM_bode_linewidth)
axis([0.01 100 0.1 5]);
ylabel('S');
xlabel('w');
Ms,Mt,gm

amprint('afm-gangof4.eps');