%Design of integral controller for an AFM in tapping mode
% kja 070404

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;
%Pure integrating control
kipure=1/(2+Tn)

% Nyquist and bode plot

%Transfer function
w=logspace(-2,2,N);
one=ones(size(w));
s=i*w;
P='(1-exp(-s*Tn))./(s.*Tn.*(s+1))'
Pv=eval(P);

%Design of PI controller with Ziegler-Nichols method
L='(kipure./s).*(1-exp(-s*Tn))./(s.*Tn.*(s+1))';
Lv=eval(L);

% 
% Nyquist plot
%
figure(1); clf; xsubplot(221);
amaxis([-1.5 1 -10 10]); hold on;

% Create a frequency response object that contains the parts we want
s = i*logspace(-1.25, 2, 1000);
sys = frd(eval(L), logspace(-1, 2, 1000));

% Draw the Nyquist plot for the main portion
AM_nyquist_plain = 1;
amnyquist(sys);
AM_nyquist_plain = 0;

% set up the color
color = 'b';
if (AMPRINT_FLAG==2) color = 'k'; end;

% Add arrows part way along the curve
mid = 50;
H1 = sys.response(mid); H2 = sys.response(mid+1)
arrow([real(H2), -imag(H2)], [real(H1), -imag(H1)], ...
  AM_normal_arrowsize, 'FaceColor', color, 'EdgeColor', color);

mid = 80;
H1 = sys.response(mid); H2 = sys.response(mid+1)
arrow([real(H1), imag(H1)], [real(H2), imag(H2)], ...
  AM_normal_arrowsize, 'FaceColor', color, 'EdgeColor', color);

% Add an arc for the pole at the origin
rx = 1.25; ry = 8;
plot(rx*cos(-pi/2:0.01:pi/2) - 0.49, ry*sin(-pi/2:0.01:pi/2), 'b-.');

% Tweak the aspect ratio
daspect([2.5 20 1]);

% Print out the stability margin
[smzn,jjj]=min(abs(1+Lv)); smzn

amprint('afmtap-inyq.eps');

%
%Bode plot
%
figure(2); clf; subplot(221);

sys = frd(Lv, w);
[magh, phaseh] = ambode(sys);

% Figure out the gain and phase margins for later use
[gm, pm, wcg, wcp] = margin(sys);

subplot(magh); hold on;
loglog(w, one, 'k', 'LineWidth', AM_ref_linewidth);
loglog([wcg wcg], [1, 1/gm], 'r-', [wcg], [1/gm], 'ro', ...
  'MarkerSize', AM_bode_markersize);

amaxis([1e-2 100 0.0001 100]);
set(gca,'Ytick',[0.0001 0.01 1 100])
ylabel('gain');

subplot(phaseh); hold on;
semilogx(w, -180*one, 'k', 'LineWidth', AM_ref_linewidth);
semilogx([wcp wcp], [-180, pm-180], 'r-', [wcp], [pm-180], 'ro', ...
  'MarkerSize', AM_bode_markersize);

% set(gca,'Ytick',[-270 -180 -90 0]);
amaxis([1e-2 100 -370 10]);
xlabel('w');
ylabel('phase');

amprint('afmtap-ibode.eps');
