clear

ymax=1.41;
tmax=0.38;

zeta=sqrt((log(ymax-1.0))^2/(pi^2+(log(ymax-1.0))^2));
omegan=pi/(tmax*sqrt(1-zeta^2));

num=[omegan^2];
den=[1 2*zeta*omegan omegan^2];


T=0.01;
F=den(3)/den(2);
a=den(2);
c=a*T-1+exp(-a*T);
d=1-exp(-a*T)-a*T*exp(-a*T);
e=exp(-a*T);

%%% open loop G(z) %%%%%%%%%%%%%%%%%%%%%%
BO=[0 F*c F*d]/a;
AO=[a -a-a*e a*e]/a;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% closed loop T(z)=G(z)/(1+G(z)) %%%%%%
BC=[0 F*c F*d]/a;
AC=[a (F*c-a-a*e) (F*d+e*a)]/a;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%% inverse bilinear transformation %%%%%%%%%%
bo=[BO(3) BO(2) BO(1)]';
ao=[AO(3) AO(2) AO(1)]';

Q=[1 1 1;-2 0 2;1 -1 1];

boT=Q*bo;
aoT=Q*ao;

BOT=[boT(3) boT(2)/(T/2) boT(1)/(T/2)^2];
AOT=[aoT(3) aoT(2)/(T/2) aoT(1)/(T/2)^2];
sysc=tf(BOT,AOT);

q=0:.1:50;
[H,W]=freqs(BOT,AOT,q);

subplot(211)
semilogx(W,20*log10(abs(H)));
grid
subplot(212)
semilogx(W,180/pi*angle(H));
grid


%%%%%%%%%%%%%%%%% DESIGN %%%%%%%%%%%%%%%%%%%%%%%%%%%%


a0=1.0;	%DC gain
phim=55;	%desired gain margin
ww1=20.0;		%phase margin frequency
gww1mag=.168;	%magnitude G(jww1)
phasegww1=-175;	%phase     G(jww1)

thetad=180+phim-phasegww1;
thetar=thetad*pi/180

a1=(1-a0*gww1mag*cos(thetar))/(ww1*gww1mag*sin(thetar));
b1=(cos(thetar)-a0*gww1mag)/(ww1*sin(thetar));

w0=1/a1;
wp=1/b1;

%%% digital controller BLT D(s)=DbT(s)/DaT(s)

DbT=[0 a1 1];
DaT=[0 b1 1];
sysd=tf(DbT,DaT);
sysol=sysc*sysd;
sysh=tf(1,1);
syscl=feedback(sysol,sysh);

%%% digital controller D(z)=Db(z)/Da(z)

Db=wp*(w0+2/T)/(w0*(wp+2/T))*[1 -(2/T-w0)/(2/T+w0)];
Da=[1 -(2/T-wp)/(2/T+wp)];
sysx=tf(Db,Da,-1)

