%Plot Gang of Four for oscillating system with PID notch control
% kja 070401
aminit					% MATLAB initialization

%Process and controller transfer functions for ideal PID controller
%Notch filter design
P=tf([1], [1 0.02 1]);			% 1/(s^2 + 0.02*s + 1)
C=0.5 * tf([1 0.02 1], [1 0]);		% 0.5 (s^2 + 0.02*s + 1) / s;
%Controller parameters
ki=0.5;k=0.5*0.02;kd=ki;Ti=k/ki;Td=kd/k;

%Compute Gang of Four Transfer funcitons
w1=-3;w2=2;N=1e4;
[T,PS,CS,S,w]=gangof4(P,C,w1,w2,N);

%Plot results
figure(1); clf;

subplot(321);
ylabel('T');
loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth); hold on;
h1 = loglog(w,abs(T),'--','Linewidth',AM_data_linewidth, 'Color', [0 0.5 0]);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 10]); hold on;
ylabel('T');

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

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

subplot(324);
loglog([0.001 1000], [1 1], 'k-', 'LineWidth', AM_ref_linewidth); hold on;
loglog(w,abs(S),'--','Linewidth',AM_data_linewidth, 'Color', [0 0.5 0]);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 10]); hold on;
xlabel('w');ylabel('S');
[ms,iii]=max(abs(S));
ws=w(iii);
[mt,jjj]=max(abs(T));
wt=w(jjj);

%Add high frequency roll-ofg in controller
%C = C * tf([0.5], [0.375^2 0.75 1]);		% 1/(1+0.75*s+(0.375*s).^2/2);
Tf=0.2/ki;
C = C * tf(1,[Tf^2/2 Tf 1]);
[T,PS,CS,S,w]=gangof4(P,C,w1,w2,N);

subplot(321);
ylabel('T');
h2 = loglog(w,abs(T),'b-','Linewidth',AM_data_linewidth);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 10]);
ylabel('T');

lgh = legend(gca, [h1 h2], 'Ideal PID', 'PID w/ filtering', ...
  'Location', 'southwest');
legend(lgh, 'boxoff');

subplot(322);
loglog(w,abs(PS),'b-','Linewidth',AM_data_linewidth);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 100]);
ylabel('PS');

subplot(323);
loglog(w,abs(CS),'b-','Linewidth',AM_data_linewidth);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 100]);
xlabel('w');ylabel('PC');

subplot(324);
loglog(w,abs(S),'b-','Linewidth',AM_data_linewidth);
set(gca,'Xtick',[0.01 1 100]);
set(gca,'Ytick',[0.01 1]);
amaxis([0.001 100 0.01 10]);
xlabel('w');ylabel('S');
[ms,iii]=max(abs(S));
ws=w(iii);
[mt,jjj]=max(abs(T));
wt=w(jjj);

amprint('afm-pidnotchgof.eps');
