%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Aufgabe 2.1
% FKL Reglerentwurf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
clc;
close all;

% Parameter laden
run('Esys_Parameter.m');

s = tf('s');
%G = tf(V/(1+2*xi*(s*T)+(s*T)^2));
%% Aufgabe 2.1.1
% Diese Struktur ist sinnvoll da:
% - Integralanteil: für die bleibende Regelabweichung von 0 zum. ein Integrierer
%   vorhanden sein muss
% - Proportionalanteil: damit das Eingangssignal verstärkt wird
%% Aufgabe 2.1.2 PI Regler

% Anforderungen an den offenen und geschlossenen Kreis
tr = 1.5e-3; % s
ue = 5; % %

% über Näherung
wc = 1.5/tr;
phi = 70-ue;
% PI ist sinnvoll, da wir für e_inf=0 einen Integrator also Polstelle bei s = 0 im offenen Kreis brauchen.

% Bode Diagramm mit bekannten Termen
close all
%hold on
PIL1 = G/tf(s);
% Auslesen aus Bode Diagramm an w = wc
[PIabs1, PIpha1] = bode(PIL1, wc);
%bode(L1)
% Es muss pha + arg(1+I*wc*Ti) = phi - 180 sein also
% Ti = tan(phi-pha-180)/wc
Ti = tan((phi-PIpha1-180)*pi/180)/wc;
PIL2 = PIL1*tf(1+s*Ti);
%bode(L2)
%hold off

% Verstärkung
[PIabs2, PIphi2] = bode(PIL2, wc);
% Abs sollte 1 sein, damit muss V = 1/abs sein
PIV = 1/PIabs2;
PIL = PIL2*tf(PIV);
[PIabs3, PIphi3] = bode(PIL, wc);

% Gesamtregler und offener Kreis
R_PI = minreal(PIL/G)
L_PI = minreal(PIL)

% Da unser offener Regelkreis nicht exakt die Form die im Skriptum
% angegeben ist hat, sind die empirischen Näherungsbedingungen nicht exakt
% sondern eben nur Näherungsbedingungen.
% Da die Strecke mit R und G zusammen kein PT2 Glied ist sind sie in diesem
% Fall nicht exakt.
%% Aufgabe 2.1.3 PID Regler
wc = 1e3
ue = 5;
phi = 70 - ue;
% Bleibende Regelabweichung
e_inf = 1e-3;

% Zeitkonstante des Intagralterms
TI = 0.25e-3;

% Da e_inf = 1e-3 für r(t) = t muss der Verstärkungsfaktor des offenen
% Kreises
VL = 1/e_inf;
VP = VL/V;

PIDL1 = G*tf((1+s*TI)*VP/s);
[PIDabs1, PIDphi1] = bode(PIDL1, wc);
% Was bleibt ist ein Lead-Glied 
global ll phaseR absR
ll = @(T) (1+1i*wc*T(1))/(1+1i*wc*T(2));
phaseR = phi-PIDphi1-180;
absR = 1/PIDabs1;

% umgeformtes Gleichungssystem lösen
TS = fsolve(@TDTR, [0; 0]);
TD = TS(1);
TR = TS(2);
PIDL = PIDL1*tf((1+s*TD)/(1+s*TR));
[PIDabs2, PIDphi2] = bode(PIDL, wc)
%bode(L)

% Gesamtregler und offener Kreis
R_PID = minreal(PIDL/G)
L_PID = minreal(PIDL)

figure(1); 
%bode(L_PI)
hold on
grid on
bode(L_PID,'r')
bode(L_PI,'b')
title('Bodeplot offener Kreis')
legend('PI','PID')
%% Aufgabe 2.1.4

%                    d->| Gd|---,
%                               |
%  r ---+-->| R |---u-->| G |---+---> y
%    (-)|                       |
%       `-----------------------'

% Kontrolle des Verhaltens des geschlossenen Kreise
% Übertragungsfunktionen
Try_PI = minreal(L_PI/(1+L_PI))
Tdy_PI = minreal(Gd/(1+L_PI))
Try_PID = minreal(L_PID/(1+L_PID))
Tdy_PID = minreal(Gd/(1+L_PID))

T = 0.015; % Simulationsdauer
t = 0:0.0001:T;
figure(2);
subplot(4,1,1)
hold on
grid on
% Sprung
plot(t, heaviside(t));
% Sprungantwort Try_PI
[y, t] = lsim(Try_PI, heaviside(t), t);
plot(t, y);
% Sprungantwort Try_PID
[y, t] = lsim(Try_PID, heaviside(t), t);
plot(t,y);
title('Sprungantwort Führungsübertragungsfunktion')
legend('r', 'PI','PID')
 
subplot(4,1,2)
hold on
grid on
% Sprung
plot(t, heaviside(t));
% Sprungantwort Tdy_PI
[y, t] = lsim(Tdy_PI, heaviside(t), t);
plot(t, y);
% Sprungantwort Tdy_PID
[y, t] = lsim(Tdy_PID, heaviside(t), t);
plot(t, y);
title('Sprungantwort Störübertragungsfunktion')
legend('d', 'PI','PID')
 
subplot(4,1,3)
hold on
grid on
% Rampe
plot(t, heaviside(t).*t);
% Rampenantwort Tdy_PI
[y, t] = lsim(Try_PI, heaviside(t).*t, t);
plot(t, y);
% Rampenantwort Tdy_PID
[y, t] = lsim(Try_PID, heaviside(t).*t, t);
plot(t, y);
title('Rampenantwort Führungsübertragungsfunktion')
legend('r', 'PI','PID')

subplot(4,1,4)
hold on
grid on
% Rampe
plot(t, heaviside(t).*t);
% Rampenantwort Tdy_PI
[y, t] = lsim(Tdy_PI, heaviside(t).*t, t);
plot(t, y);
% Rampenantwort Tdy_PID
[y, t] = lsim(Tdy_PID, heaviside(t).*t, t);
plot(t, y);
title('Rampenantwort Störübertragungsfunktion')
legend('d', 'PI','PID')
%% Aufgabe 2.1.5
% Anforderungen werden erfüllt. Das Überschwingen und die bleibende
% Regelabweichung werden eingehalten, auch die Ansteigszeit ist laut
% `stepinfo` bei 1.5ms (PI) und 1.6ms (PID) jeweils.
Try_PI_stepinfo  = stepinfo(Try_PI)
Try_PID_stepinfo = stepinfo(Try_PID)
%% Aufgabe 2.1.6
% Die Stellgrößen und damit auch der Ausgang des Reglers und anschließend
% des Systems hat kleine Abweichungen aufgrund des Messfehlers. Das
% Messrauschen wird aber nicht allzu stark verstärkt.
%% Aufgabe 2.1.7
% Der Regler tut sich leichter dem Verlauf der Eingangsspannung zu folgen
% und die Anstiegszeit wird somit eher eingehalten. Allgemein sind die
% Anforderungen eines Reglers leichter zu erfüllen je mehr Zeit durch die
% Begrenzung der Änderungsrate geschafft wird.
% Bei der Stellgröße gibt es dafür keinen so großen Sprung beim Aufschalten
% eines Impulses, was womöglich gut für die Komponenten in der Schaltung
% ist. Bei der OPV Schaltung ergibt so eine Begrenzung auch Sinn, weil sie
% keine ideale Spannungsversorgung haben.
%% Pol-/Nullstellenanalyse
figure(3);
subplot(2,2,1);
hold on
pzmap(Try_PI)
title('Pole Zero PI ry')

subplot(2,2,2);
hold on
pzmap(Try_PID)
title('Pole Zero PID ry')

subplot(2,2,3);
hold on
pzmap(Tdy_PI)
title('Pole Zero PI dy')

subplot(2,2,4);
hold on
pzmap(Tdy_PID)
title('Pole Zero PID dy')

figure(4);
subplot(1,2,1);
hold on
pzmap(R_PI)
title('Pole Zero RPI')

subplot(1,2,2);
hold on
pzmap(R_PID)
title('Pole Zero RPID')

PIp = pole(Try_PI)
PIz = zero(Try_PI)
PIDp = pole(Try_PID)
PIDz = zero(Try_PID)
%% function definitions
function F = TDTR(T)
    global ll phaseR absR
    eq1 = rad2deg(angle(ll(T))) - phaseR;
    eq2 = abs(ll(T)) - absR;
    F = [eq1; eq2];
end