%Design of PID controller with active damping
%  kja 070402

aminit					% MATLAB initialization

w=logspace(-2,3,1000);
s=i*w;

%Process transfer function
a=1;z=0.01;
P=a^2./(s.^2+2*z*s+a^2);

%Desired closed loop poles
Str={'-.','--','-'};
Col={'b', [0 0.5 0], 'r'}
W0=[1 2 4];
z0=0.707;alpha0=1;

figure(1); clf;
for ii=1:3,
  w0=W0(ii);    
  str=Str{ii};
  col=Col{ii};

  %z0=0.7;alpha0=3;
  kp=(1+2*alpha0*z0)*w0^2/a^2-1;
  ki=alpha0*w0^3/a^2;
  %kd=((alpha0+2*z0)*w0-2*z*a)/a^2
  kd=(alpha0+2*z0)*w0/a^2-2*z*a;
  Td=kd/kp;Tf=Td/10;
  %[kp ki kd Td];
  %roots([kd kp ki])
  C=(kp+ki./s+kd*s)./(1+s*Tf+s.^2*Tf^2/2);

  %Gang of Four
  L=P.*C;
  S=1./(1+L);
  PS=P.*S;
  CS=C.*S;
  T=L.*S;

  %Plot results
  subplot(321);
  ylabel('T');
  loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
  loglog(w,abs(T), str, 'Linewidth', AM_data_linewidth, 'Color', col);
  set(gca,'Xtick',[0.01 1 100]);
  set(gca,'Ytick',[0.01 1]);
  amaxis([0.01 1000 0.001 10]); hold on;
  ylabel('T'); hold on

  subplot(322);
  loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
  loglog(w,abs(PS),str,'Linewidth',AM_data_linewidth, 'Color', col);
  set(gca,'Xtick',[0.01 1 100]);
  set(gca,'Ytick',[1e-4 0.01 1]);
  amaxis([0.01 1000 0.0001 1]); hold on;
  ylabel('PS');hold on

  subplot(323);
  loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
  loglog(w,abs(CS),str,'Linewidth',AM_data_linewidth, 'Color', col);
  set(gca,'Xtick',[0.01 1 100]);
  set(gca,'Ytick',[1 100]);
  amaxis([0.01 1000 0.2 2000]); hold on;
  xlabel('w');ylabel('PC');

  subplot(324);
  loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth);
  loglog(w,abs(S),str,'Linewidth',AM_data_linewidth, 'Color', col);
  set(gca,'Xtick',[0.01 1 100]);
  set(gca,'Ytick',[0.01 1]);
  amaxis([0.01 1000 0.001 10]); hold on;
  xlabel('w');ylabel('S');

  % Gain crossover frequency
  [xxx,iii]=min(abs(abs(L)-1));
  wgc=w(iii);
  % Maximum sensititivites
  [ms,iii]=max(abs(S));
  ws=w(iii);
  [mt,jjj]=max(abs(T));
  wt=w(jjj);
  [mcs,jjj]=max(abs(CS));
  wt=w(jjj);
  [wgc mcs ms mt]
end

subplot(321); hold on;
lgh = legend('w0 = a', 'w0 = 2a', 'w0 = 4a', 'Location', 'southwest');
legend(lgh, 'boxoff');

amprint('afm-piddampgof.eps');


