% Projekt SA-Lab Ćwiczenie 7
% Filipów Bartosz 160 488
% Pełka   Jakub   160 648

clc; clear;
% Parametry układu
Tp = 1.2e-3;
Kp = 1.31;
a1 = 4.1918e-4;
a2 = 1.4037e-7;
Ti = 3.045e-3;
T0 = 212.500e-6;

% Transmitancja Sterownika
Lc = [0.09];
Mc = [1];
Gc = tf(Lc,Mc);
[Ac,Bc,Cc,Dc] = ssdata(Gc);

% Transmitancja Obiektu
Lp = [Kp];
Mp = [Ti*Tp*a2 (Ti*Tp*a1+Ti*a2) (Ti*Tp +Ti*a1) Ti 0];
Gp = tf(Lp,Mp);
[Ap,Bp,Cp,Dp] = ssdata(Gp);

% Transmitancja Czujnika
Lt = [1];
Mt = [10*Tp 1];
Gt = tf(Lt,Mt);
[At,Bt,Ct,Dt] = ssdata(Gt);

% Jednostkowe sprzężnie zwrotne
Lj = [1];
Mj = [1];
Gj = tf(Lj,Mj);
[Aj,Bj,Cj,Dj] = ssdata(Gj);

% Połączenie szeregowe - układ otwarty
Trans = series(Gc,Gp);
[Ao,Bo,Co,Do] = ssdata(Trans);
figure(1)
rlocus(Ao,Bo,Co,Do);

% Ujemne Sprzężenie zwrotne z czujnikiem
[A,B,C,D] = feedback(Ao,Bo,Co,Do,At,Bt,Ct,Dt,-1);

% Stabilność układu
[P,Z]   = pzmap(Ao,Bo,Co,Do)    % Bieguny i zera układu

% SYMULACJA
Umax    = 2;                     % Ograniczenie sygnału
TopO    = 2*T0;                  % Opóźnienie obiektu
TopC    = 0;                     % Opóźnienie czujnika
dT      = 1e-6;
Tsym    = 0.5;
Ur      = 1;

Xp  = zeros(size(Bp));           % Wektor stanu
Xt  = zeros(size(Bt));
Xc  = zeros(size(Bc));

Ap  = dT*Ap; Bp = dT*Bp;         % Modyfikacja modelu
At  = dT*At; Bt = dT*Bt;
Ac  = dT*Ac; Bc = dT*Bc;


Ts  = 0:dT:Tsym;                 % Wektor czasu

N   = length(Ts);

R   = ones(size(Ts));            % Sygnał zadający
U   = zeros(size(Ts));           % Sygnał sterujący
Uop = zeros(size(Ts));           % Sygnał sterujący opóźniony
Ucz = zeros(size(Ts));           % Sygnał z czujnika opóźniony

Ys  = zeros(size(Ts));           % Sygnał wyjścia obiektu
Yt  = zeros(size(Ts));           % Sygnał wyjścia czujnika
Yc  = zeros(size(Ts));           % Sygnał wyjścia sterownika

E   = zeros(size(Ts));           % Sygnał uchybu

% Tablice opóźnień
NopO = TopO/dT;                  % Obiektu
NopO = round(NopO);

NopC = TopC/dT;                  % Czujnika
NopC = round(NopC);

for t=1:N-1,
    % Przewidywanie przyszłego wyjścia sterownika
    X2c     = Xc + Ac*Xc + Bc*E(t+1);
    Yc(t+1) = Cc*X2c + Dc*E(t+1);

    % Przewidywanie przyszłego wyjścia obiektu
    Xp2     = Xp + Ap*Xp + Bp*Uop(t+1);
    Ys(t+1) = Cp*Xp2 + Dp*Uop(t+1);

    % Przewidywanie przyszłego wyjścia czujnika
    X2t     = Xt + At*Xt + Bt*Ys(t+1);
    Yt(t+1) = Ct*X2t + Dt*Ys(t+1);

    % Uchyb
    E(t)    = R(t)  - Ucz(t);
    E(t+1)  = R(t+1)- Ucz(t+1);

    % Symulacja dynamiki sterownika
    Yc(t)   = Cc*Xc + Dc*E(t);
    X2c     = Xc + Ac*Xc +Bc*E(t+1);
    Xc      = Xc + 0.5*(Ac*Xc + Bc*E(t) + Ac*X2c + Bc*E(t+1));
    Yc(t+1) = Cc*Xc + Dc*E(t+1);

    U(t)    = Yc(t);
    U(t+1)  = Yc(t+1);

    % Ogrniczenie wartości syngału sterującego
    if U(t)<-Umax,
        U(t) = -Umax;
    end;
    if U(t)>Umax,
        U(t) = Umax;
    end;
    if U(t+1)<-Umax,
        U(t+1) = -Umax;
    end;
    if U(t+1)>Umax,
        U(t+1) = Umax;
    end;

    % Opóźnienie wejścia obiektu
    if t==NopO
        Uop(t+1) = U(t+1-NopO);
    end;
    if t>NopO
        Uop(t)   = U(t-NopO);
        Uop(t+1) = U(t+1-NopO);
    end;

    % Symulacja dynamiki obiektu
    Ys(t)   = Cp*Xp + Dp*Uop(t);
    Xp2     = Xp + Ap*Xp + Bp*Uop(t);
    Xp      = Xp + 0.5*(Ap*Xp + Bp*Uop(t) + Ap*Xp2 + Bp*Uop(t+1));
    Ys(t+1) = Cp*Xp + Dp*Uop(t+1);

    % Symulacja dynamiki czujnika
    Yt(t)   = Ct*Xt + Dt*Ys(t);
    X2t     = Xt + At*Xt + Bt*Ys(t+1);
    Xt      = Xt + 0.5*(At*Xt + Bt*Ys(t) + At*X2t + Bt*Ys(t+1));
    Yt(t+1) = Ct*Xt + Dt*Ys(t+1);

    % Opóźnienie czujnika
    if t==NopC
        Ucz(t+1) = Yt(t+1-NopC);
    end;
    if t>NopC
        Ucz(t)   = Yt(t-NopC);
        Ucz(t+1) = Yt(t+1-NopC);
    end
end;

figure(2)
step(A,B,C,D);
hold on;
plot(Ts,Ys,'r');
hold off;
legend('Bez opóźnienia za pomocą step','Pętla for z opóźnieniem');
grid on;
title('Odpowiedz skokowa układu');

% Obliczanie wskaźników jakości układu
T5  = 0;
T2  = 0;
K   = 0;

for i = 1:length(Ys)
    if abs(Ur- Ys(i)) < 0.05*Ur && T5 == 0
        T5 =i;
    end
    if abs(Ur- Ys(i)) >0.05*Ur
        T5 = 0;
    end

    if abs(Ur- Ys(i)) < 0.02*Ur && T2 == 0
        T2 = i;
    end
    if abs(Ur- Ys(i)) >0.02*Ur
        T2 = 0;
    end

    if Ys(i) > K
        K = Ys(i);
    end
end

T5 = T5*dT;
T2 = T2*dT;
ep = Ur - Ys(length(Ys));
if K > Ur
    K = ((K-Ur)/Ur)*100;
else
    K = 0;
end

disp(['T5% = ' num2str(T5) '   T2% = ' num2str(T2) '   Przeregulowanie = ' num2str(K) '   Uchyb pozycyjny = ' num2str(ep)]);