close all


% errore finale alla rampa unitaria <= 0,05 secondi
% picco di risonanza della f.d.t. T(s): Mr <= 2 dB (Phi_m>=46°)
% banda passante wB compresa tra 4 e 6 rad/s

s=tf('s');
% funzione di trasferimento in forma costanti di tempo
G=1/(s+1)/(1+s/20);
% guadagno stazionario mutilde=1
% errore alla rampa 1/mu < 0.05 => 
Kc=1/0.05;
Cb=Kc/s;  %polo nell'origine (il sistema deve essere di tipo 1)

% banda passante wB compresa tra 4 e 6 rad/s
wb_min=4;
wb_max=6;
wc_min=0.4*wb_min;
wc_max=0.8*wb_max;
wc_des=(wc_max+wc_min)/2 %  puls. di taglio desiderata

% specifica su  errore di riproduzione di segnali sinusoidali con w<0.1 rad/s 
% minore dell'1% => |L|>100 (=40dB) per w<0.1

figure(1),
semilogx(wc_des,0,'*r'),hold on,grid on
fill([0.01,0.1,0.1,0.01],[40,40,-100,-100],'g')
axis([0.01,100,-100,100]);

% specifica su attenuazione disturbi sinusoidali con w>20 rad/s 
% minore del 5% => |L|<0.05 (=-26dB) per w>20

fill([20,100,100,20],[-26,-26,100,100],'g')
axis([0.01,100,-100,100]);

disp('---');pause

% costruisco la prima funzione di anello (dopo l'introduzione del
% compensatore statico
L1=Cb*G

w=logspace(-2,2,100);figure(1),hold on 
bode(L1,w),axis([0.01,100,-270,-90]),grid on

% calcolo modulo e fase alla freq. di taglio desiderata
[mod_L1,fase_L1]=bode(L1,wc_des)

disp('---');pause

% fase da recuperare (recupero 5 in più per tener conto della
% integratrice
phi_m=45
phi_d=-180-fase_L1+phi_m+5
phi=phi_d/180*pi;

alpha_d=(1+sin(phi))/(1-sin(phi))

disp('---');pause

% scelgo alpha_d=5
alpha_d=5;
%massimo recupero in sqrt(alpha_d*wd) - o faccio coincidere con wc_des
wd=wc_des/sqrt(alpha_d);
% rete derivatrice
Cd=(1+s/wd)/(1+s/(alpha_d*wd))
figure(2),bode(Cd),grid on

disp('---');pause

% funzione di anello dopo l'introduzione della derivatrice
L2=(Cb*Cd)*G;
figure(1),hold on,bode(L2,w)

disp('---');pause

%calcolo modulo e fase di L2 in wc_des=3.2
[mod_L2,fase_L2]=bode(L2,wc_des)

% devo mettere un'integratrice che alzi il modulo di mod_L2
alpha_i=mod_L2;
% metto lo zero (alpha_i*wi) una decade prima di wc_des
wi=wc_des/(10*alpha_i);
Ci=(1+s/(alpha_i*wi))/(1+s/wi);
figure(2),bode(Ci),grid on

% funzione di anello dopo l'introduzione della integratrice
L3=(Cb*Cd*Ci)*G;
figure(1),hold on,bode(L3,w)

%costruisco il compensatore finale C(s)
C=Cb*Cd*Ci;
disp('---');pause
% costruisco la funzione si sensitività complementare
figure(2),sigma(L3),grid on,hold on
T=feedback(L3,1);
figure(2),sigma(T),grid on %sigma disegna solo il modulo
figure(3),step(T)
disp('---');pause
%wB=5.69
% costruisco la funzione si sensitività
S=feedback(1,L3);
figure(2),hold on,sigma(S)
disp('---');pause
% costruisco la funzione si sensitività del controllo
Tc=feedback(C,G);
figure(2),hold on,sigma(Tc)
figure(3),hold on,step(Tc)

%La banda passante è circa 6rad/s
wb=6;

%------------ discretizzazione (1)
%Scelgo il tempo di campionamento -- ws=10wb --
ws=10*wb 
fs=ws/(2*pi);
Ts1=1/fs


% calcolo la perdita di fase alla frequenza wc
Ho1=1/(1+s*Ts1/2);
[mod_Ho1,fase_Ho1]=bode(Ho1,wc_des)
figure(1)
bode(Ho1*L3)
disp('---');pause

% discretizzo G (tenendo conto dello zoh)
G_d1=c2d(G,Ts1,'zoh');
zpk(G_d1);
% discretizzo C (usando Tustin)
C_d1=c2d(C,Ts1,'tustin');
disp('C_d1');zpk(C_d1)

L_d1=C_d1*G_d1;
figure(1)
bode(L_d1),
T_d1=feedback(L_d1,1);
figure(3)
step(T_d1)
disp('---');pause


%------------ discretizzazione (2)
%Scelgo il tempo di campionamento -- ws=100wb --
ws=100*wb 
fs=ws/(2*pi);
Ts2=1/fs


% calcolo la perdita di fase alla frequenza wc
Ho2=1/(1+s*Ts2/2);
[mod_Ho2,fase_Ho2]=bode(Ho2,wc_des)
figure(1)
bode(Ho2*L3)
disp('---');pause

% discretizzo G (tenendo conto dello zoh)
G_d2=c2d(G,Ts2,'zoh');
zpk(G_d2)
% discretizzo C (usando Tustin)
C_d2=c2d(C,Ts2,'tustin')
zpk(C_d2)

figure(1)
L_d2=C_d2*G_d2;
bode(L_d2),
T_d2=feedback(L_d2,1)
figure(3)
step(T_d2)
% suppongo ci sia quantizzazione (1)
z=tf('z',Ts1);
C_d1_quant=1.01*(z+1)*(z-0.97)*(z-0.86)/( (z-1)*(z-0.99)*(z-0.45));
disp('C_d1_quant');zpk(C_d1_quant)
L_d1_quant=C_d1_quant*G_d1;
figure(1)
bode(L_d1_quant),
T_d1_quant=feedback(L_d1_quant,1);
figure(3)
step(T_d1_quant)
disp('---');pause

% suppongo ci sia quantizzazione (2)
z=tf('z',Ts2);
C_d2_quant=0.12*(z+1)*(z-0.99)*(z-0.98)/((z-1)*(z-0.99)*(z-0.93));
disp('C_d2_quant');zpk(C_d2_quant)
L_d2_quant=C_d2_quant*G_d2;
figure(1)
bode(L_d2_quant),
T_d2_quant=feedback(L_d2_quant,1);
figure(3)
step(T_d2_quant)
disp('---');pause