% piezobode.m Bode plots for AFM z-positioner
% kja 060806
%Fitting of models
%
aminit;

f=logspace(2,4,100);w=2*pi*f;s=i*w;

%KJs manual fit
w1 = 2*pi*8250; z1 = 0.025;
w2 = 2*pi*6450; z2 = 0.042;
w3 = 2*pi*9300; z3 = 0.032;
w5 = 2*pi*2420; z5 = 0.030;
w6 = 2*pi*2550; z6 = 0.03;
T = 50e-6; K = 5;

P1=w1^(-2)*(s.^2+2*z1*w1*s+w1.^2);
P2=w2^2./(s.^2+2*z2*w2*s+w2.^2);
P3=w3^2./(s.^2+2*z3*w3*s+w3.^2);
P4=1./(1+s*T/2).^2;
P5=w5^(-2)*(s.^2+2*z5*w5*s+w5.^2);
P6=w6^2./(s.^2+2*z6*w6*s+w6.^2);
P=K*P1.*P2.*P3.*P4.*P5.*P6;
k=0.5;Ti=0.00005;C=k*(1+1./s/Ti)./(1+s*Ti/5).^2;
one=ones(size(f));

%Controller transfer function
k=1;Ti=1/(2*pi*3000);
%C=k*(1+1./(s*Ti));
ki=2*pi*1000;C=ki./s;

%Load experimental data
filename=['afm-piezobode.dat'];
data=load(filename);
x=data(:,1);
y=data(:,2)+ j*data(:,3);

% Generate a set of axes that we can use 
figure(1); clf; 
% axes('position', [0.13 0.4096 0.3347 0.5]);
axes('position', [0.13 0.4096 0.4 0.5]);
[magh, phaseh] = ambode(frd(-y, x));

%Plot
subplot(magh); hold on;
loglog(f, abs(P), 'r--', 'Linewidth', AM_thick_linewidth);
loglog(x, abs(y), 'b-', 'Linewidth', AM_bode_linewidth);

% set(gca,'Ytick',[1 10])
% amaxis([100 1e4 0.5 30]); box on;

lgh = legend('Measured', 'Model', 'Location', 'southwest');
legend(lgh, 'boxoff');

subplot(phaseh); hold on;
semilogx(f, 180*unwrap(angle(P))/pi, 'r--', 'Linewidth', AM_thick_linewidth);
semilogx(x, 180*unwrap(angle(y))/pi-180, 'b-', 'Linewidth', AM_bode_linewidth);

set(gca, 'Ytick', [-300 -200 -100 0]);
set(gca, 'XTick', [100 1000 10000]);

ylabel('Phase'); xlabel('Frequency Hz');
set(gca,'Ytick',[-270 -180 -90 0])
amaxis([100 1e4 -300 50])

amprint('afm-piezobode.eps');
