% robustni regulator

clear;
close all;

% diskretni system, zadane hodnoty
S_b = [0.0931 0.5552 0.1806 -0.2692 -0.03966];
S_a = [1 -0.5404 -0.8094 -0.05273 0.7354 -0.3329];
Ts = 0.1;
P=1.5;

S = tf(S_b, S_a, Ts);

% mam tam jeden koren p=(1+0j), muzu pouzit Pcko
roots(S_a);

% nichols
figure(1);
ngrid;
nichols(S);
% nyquist
figure(2);
nyquist(S);

% vahova funkce
w=(0:0.01*pi:pi*10);
[M,Ph,w] = bode(S,w);
MS(1,:)=M(:,:,:);
W2=abs((P./MS)+1);
i=find(w>0.7*pi);
W2_1=W2(i:size(W2,2));
W2=W2_1;

%--------------------

%REGULATOR P
K=0.35;
P_S = series(S,K);
figure(3);
hold on;
ngrid;
nichols(S);nichols(P_S);%nicholsuv diagram pro P regulator a Soustavu
hold off;

%frekvencni charakteristiku P-Soustava
CL_PS = feedback(P_S,1,-1);
[M_PS] = bode(CL_PS,w);
B_PS(1,:) = M_PS(:,:,:);
figure(4);
hold on;
plot(w(i:size(w,1)),1./W2,'b');
plot(w,B_PS,'g');

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