%Design of PID controllers with Ziegler-Nichols tuning
%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

% Normalized averaging time

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

%Nyquist plot
figure(1); clf; xsubplot(221);
ampolezero(tf([1], [1]), [-1.5 1.5 -1 1.2]);
plot(-1, 0, 'b+', 'MarkerSize', AM_bode_markersize);

% Plot the process transfer function
h1 = plot(real(Pv),imag(Pv), ':', 'LineWidth', AM_data_linewidth);
xlabel('real');ylabel('imag');hold on
amaxis([-1.9 1.5 -1 1.2]); set(gca, 'DataAspectRatio', [1 1 1]);
[xxx,iii]=min(abs(angle(Pv(1:round(0.8*N)))+pi));
plot(real(Pv(iii)), imag(Pv(iii)), 'ro', 'markersize', AM_data_markersize);

%Design of PI controller with Ziegler-Nichols method
kc=1/abs(Pv(iii));
Tc=2*pi/w(iii);
k=0.4*kc;Ti=0.8*Tc;ki=k/Ti;
kzn=k
kizn=ki
L='(k+ki./s).*(1-exp(-s*Tn))./(s.*Tn.*(s+1))';
Lv=eval(L);
h2 = plot(real(Lv),imag(Lv), 'b--', 'LineWidth', AM_data_linewidth);
[smzn,jjj]=min(abs(1+Lv));smzn

%Alternative tuning rule
k=0.16*kc;kappa=1/kc;
%Ti=Tc/(1+kappa);ki=k/Ti;
Ti=Tc/(1+4.5*kappa);ki=k/Ti;
ktk=k
kitk=ki
kitk=(0.16*kc+0.72)/Tc
L='(k+ki./s).*(1-exp(-s*Tn))./(s.*Tn.*(s+1))';
Lv=eval(L);
h3 = plot(real(Lv),imag(Lv), 'b-', 'LineWidth', AM_data_linewidth);
[smtk,jjj]=min(abs(1+Lv));smtk

lgh = legend(gca, [h1 h2 h3], 'Process', 'Zeigler-Nichols', 'Modified ZN', ...
  'Location', 'northwest');
legend(lgh, 'boxoff')

amprint('afmtap-nyq.eps');

% break

%Bode plot
figure(2); clf; subplot(421);
loglog(w, abs(Pv), 'b', 'LineWidth', AM_data_linewidth);
hold on;
%loglog(w,one,'b');
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1])
axis([1e-2 100 0.0001 2]);
ylabel('gain');


subplot(423);
semilogx(w, 180*unwrap(angle(Pv))/pi, 'b', 'LineWidth', AM_data_linewidth);
hold on;
semilogx(w,-180*one,'b');
%set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[-270 -180 -90 0]);
axis([1e-2 100 -280 10]);
xlabel('w');
ylabel('phase');

amprint('afmtap-bode.eps');
