%ROBUSTNI STABILITA

close all;
clear;

%zadani diskretni soustavy
Soustava_Cit_Z = [0.0931 0.5552 0.1806 -0.2692 -0.03966];
Soustava_Jmn_Z = [1 -0.5404 -0.8094 -0.05273 0.7354 -0.3329];
Soustava = tf(Soustava_Cit_Z,Soustava_Jmn_Z,1);
%nyquistova charakteristika Soustavy v komplexni rovine
figure(1);
nyquist(Soustava);
%nicholsuv diagram Soustavy
figure(2);
ngrid('new');
nichols(Soustava);


%VAHOVA FUNKCE
omega=(0:0.001*pi:pi);
[Modul,Faze,omega]=bode(Soustava,omega);
Soustava_Modul(1,:)=Modul(:,:,:);
%PmaxdB=5; %dB
%10^(PmaxdB/20);
Pmax=1.5;
W2=abs((Pmax./Soustava_Modul)+1);
ind=find(omega>0.7*pi);
W2(1:ind-1)=0;
W2=W2(ind:size(W2,2));

%REGULATOR P
K=0.4;
P_Soustava = series(Soustava,K);
figure(3);
ngrid('new');
hold on;
nichols(Soustava);nichols(P_Soustava);%nicholsuv diagram pro P regulator a Soustavu
hold off;

%frekvencni charakteristiku P-Soustava
CL_PS = feedback(P_Soustava,1,-1);
[Modul_PS] = bode(CL_PS,omega);
Bode_PS(1,:) = Modul_PS(:,:,:);
figure(4);
hold on;
plot(omega(ind:size(omega,1)),1./W2,'b');
plot(omega,Bode_PS,'g');

%prechodova charakteristika s P regulatorem
figure(5);
step(CL_PS);


%REGULATOR PD
r1 = -0.3;
r0 = 0.65;
Reg_Cit = [r1+r0 -r1];
Reg_Jmn = [1 0];
Regulator=tf(Reg_Cit,Reg_Jmn,1);

PD_Soustava = series(Soustava,Regulator);
figure(6);
ngrid('new');
hold on;
nichols(Soustava);nichols(PD_Soustava);%nicholsuv diagram pro P regulator a Soustavu
hold off;

%frekvencni charakteristiku PD-Soustava
CL_PDS = feedback(PD_Soustava,1,-1);
[Modul_PDS] = bode(CL_PDS,omega);
Bode_PDS(1,:) = Modul_PDS(:,:,:);
figure(7);
hold on;
plot(omega(ind:size(omega,1)),1./W2,'b');
plot(omega,Bode_PDS,'g');

%prechodova charakteristika s PD regulatorem
figure(8);
step(CL_PDS);


%POROVNANI OBOU REGULATORU
figure(9);
hold on;
plot(omega(ind:size(omega,1)),1./W2,'b');
plot(omega,Bode_PS,'r');
plot(omega,Bode_PDS,'g');
hold off;