
%% Inicjalizacja
close all   % zamykamy wszystkie okna jesli sa otwarte
clear all   % czyscimy wszystkie zmienne jesli istnieja
clc         % czyscimy okno Matlaba
s=tf('s');  % zmienna symboliczna do definowania transmitancji 

%% Obiekt
G=400/(s*(s^2+30*s+200));  % transmitancja obiektu dla pierwszego przykladu
%G=10/(s*(s+5));   % transmitancja obiektu dla drugiego przykladu

%G=280*(s+0.5)/(s*(s+0.2)*(s+5)*(s+70));
%G=1/((2*s+1)*(s+1)*(0.5*s+1))
%% Wymagania jakosciowe zadane przez uzytkownika
PM=input('Podaj wymagany zapas fazy (w stopniach), PM = '); % zapas fazy (w stopniach)

omega_gc=input('Podaj czestotliwosc graniczna fazy (rad/sec), omega_gc = '); % czestotliwosc graniczna fazy [rad/sec]

ess_step=input('Podaj blad w stanie ustalonym dla skoku jednostkowego (w %), e_ss_step = ');
ess_ramp=input('Podaj blad w stanie ustalonym dla skoku jednostkowego (w %), e_ss_ramp = ');


%% Wyznaczenie wzmocnienia kompensatora
if ess_step>0
    ess_step=ess_step/100;  % bo uzytkownik podal wartosc w procentach
    Kp=1/ess_step;          % stala uchybu 
    Kc=Kp/dcgain(minreal(G))
elseif ess_ramp>0
    ess_ramp=ess_ramp/100;  % bo uzytkownik podal wartosc w procentach
    Kv=1/ess_ramp;          % stala uchybu predkosciowego
    Kc=Kv/dcgain(minreal(s*G))
else
    error('Bledna wartosc bledu w stanie ustalonym')
end 
%%
[Mg,Theta,W] = bode(G,omega_gc); % wywoalanie funkcji 'bode' z dwoma parametrami
                                  % wejsciowymi i trzema parametrami wyjsciowymi 
                                  % nie rysuje wykresu bodego ale zwraca
                                  % wartosci wzmocnienia (Mg), przesuniecia
                                  % fazowego (Theta) dla zadanego wektora czestotliwosci (W=omega_gc) 
                                  % Jesli wektor czestotliwosci bedzie
                                  % jednoelementowy (czyli tylko czestotliwosc graniczna fazy)
                                  % to wyliczymy wzmocnienie (Mg) i
                                  % przesuniecie fazowe wlasnie dla
                                  % omega_gc

PM_r=PM*pi/180;       % zapas fazy w radianach 
Theta_r=Theta*pi/180; % przesuniecie fazowe w radianach
                      % bo funkcje sin i cos w Malabie korzystaja z
                      % radianow a nie stopni

%% Wyznaczenie parametrow kompensatora                       
tau_z=(1+Kc*Mg*cos(PM_r-Theta_r))/(-omega_gc*Kc*Mg*sin(PM_r-Theta_r))
tau_p=(cos(PM_r-Theta_r)+Kc*Mg)/(omega_gc*sin(PM_r-Theta_r))

C=Kc*(1+s*tau_z)/(1+s*tau_p);  % transmitancja naszego kompensatora

%% Rysowanie wynikow projektowania kompensatora


 bode(G,G*C);  % wykres bodego ukladu bez i z kompensatorem
 grid on;      % siatka widoczna na wykresie
 legend('bez kompensatora', 'z kompensatorem') % dodajemy legende do wykresu
 
 figure
 step(minreal(G/(1+G)),minreal(G*C/(1+G*C))); % odpowiedz skokowa ukladu zamknietego bez i z kompensatorem
 grid on;                   % siatka widoczna na wykresie
 legend('bez kompensatora', 'z kompensatorem') % dodajemy legende do wykresu
  
%% Export do narzedzia SISOTool
sisotool(G,C); % Uruchamiamy narzedzie SISOTool dla transmitancji obiektu G i
                % transmitancji kompensatora C