clear all;
close all;

%   parametry
s=tf('s');
R1=2.03421;
H1=11;
c1=3.3665;
a2=1.86737;
b2=12.1383;
H2=17;
c2=1.94365;
q=[0:0.001:8.3];
h2=(q/c2).^2;
GM=9.1086;
gmP=4.5543;
gmPI=4.09887;
gmPID=5.446516;

%   charakterystyka statyczna
%figure
%plot(q,h2)
%title('Charakterystyka statyczna')
%xlabel('q[m^3/s]')
%ylabel('h2[m]')
%grid on
%hold on

%   punkt pracy i stałe czasowe
h20=8.5;
q0=5.66666425;
h10=2.83333426;
k=3;
T1=13;
T2=17;
T3=0.1*T1;

%   transmitancja
K=k/((T1*s+1)*(T2*s+1));
Kw = 1/(T3*s+1);
Kr=1;                                          %    transmitancja regulatora P
Ko=K*Kw*Kr;
Kz=Ko/(1+Ko);                                  %    transmitancja ukladu zamknietego
[gm,pm] = margin(Ko);

Kr=gm;                                          %    transmitancja regulatora P
Ko=K*Kw*Kr;
Kz=Ko/(1+Ko);

KrP=gmP;
KoP=K*Kw*KrP;
KzP=KoP/(KoP+1);                                  %    transmitancja ukladu zamknietego
[Pgm,Ppm] = margin(KoP);


TiPI=15.8333;
KrPI=gmPI*(1+(1/(TiPI*s)));
KoPI=K*Kw*KrPI;
KzPI=KoPI/(KoPI+1);                                  %    transmitancja ukladu zamknietego
[PIgm,PIpm] = margin(KoPI);

TiPID=9.5;
TdPID=2.375;
TfPID=0.02375;
KrPID=gmPID*(1+(1/(TiPID*s))+((TdPID*s)/(TfPID*s+1)));
KoPID=K*Kw*KrPID;
KzPID=KoPID/(KoPID+1);                                  %    transmitancja ukladu zamknietego
[PIDgm,PIDpm] = margin(KoPID);

figure
step(KzP,'r')
hold on
step(KzPI,'g')
hold on
step(KzPID,'b')


stepinfo(KzP)
stepinfo(KzPI)
stepinfo(KzPID)

figure
nyquist(KoP,'r')
hold on
nyquist(KoPI,'g')
hold on
nyquist(KoPID,'b')
hold on

%   char Nyquista
%figure
%nyquist(Ko);

%   na granicy stabilnosci
%figure
%step (Kz,200);
%title('Na granicy stabilności Kr=9,1086')