% Aufgabe 2.5
% GSM mit Propeller
clc;
clear;
close all;
%% 2.5
% FKL (im q-Bereich)

% altes File laden
run('GSM_Parameter.m');

s = tf('s');
q = s;
% reduziertes System veränderte Matrizen
credl = [0 0 1];
dredl = [0 0];

sys = ss(Aredl, Bredl, credl, dredl);
G = tf(sys);

% sampling time
Ta = 10e-3; % s
W0 = 2/Ta;
% Parameter des Entwurfs
% Bleibende Regelabweichung bei Sprung = 0
tr = 1; % s
ue = 0; % %, Überschwingsobergrenze
% Stellgrößenbeschränkung 0 <= ugsm <= 12V
% über Näherung
Wc = 1.2/tr;
if (Wc > 0.2*W0)
    print("Wc is to big")
end
phi = 70-ue;

Gz = c2d(G(1),Ta,'zoh');
Gq = d2c(Gz, 'tustin')
Gqfactored = zpk(Gq)

Gsys = tf2ss(Gz.num{1}, Gz.den{1})

% vorgegebener Regler
% R(q) = VI*(1+q*TI)/q * (1+2*xi*(q*T)+(q*T)^2)/((q-qr1)*(q-qr2))

% Wahl der Pole
Gzero = zero(Gq);
Gpoles = pole(Gq);
Rnsp = tf((q-Gpoles(1))*(q-Gpoles(2))) % Nullstellenpaar des Reglers
% Realisierungspole beliebig wählen, sodass das System stabil bleibt
%qr1 = G_zero(2)-0.1;
%qr2 = G_zero(3)-0.1;

qr1 = -6.8;
qr2 = -6.8;
Rpsp = tf((q-qr1)*(q-qr2))
%% Regler Design Teil 1
Rkomp = Rnsp/Rpsp;
R1 = Rkomp*1/q;
L1 = Gq*R1;
L1zpk = zpk(minreal(L1))
[~, pha] = bode(L1, Wc)
%bode(L1)
% Es muss pha + arg(1+I*wc*Ti) = phi - 180 sein also
% Ti = tan(phi-pha-180 [rad])/Wc
Ti = tan((phi-pha-180)*pi/180)/Wc;
%% Regler Design Teil 2
R2 = R1*(1+q*Ti);
L2 = Gq*R2;
[abs, ~] = bode(L2, Wc);
Vi = 1/abs;
%% Regler Design Final
% PI mit Kompensationsterm
R = R2*Vi;
L = Gq*R
[abs, pha] = bode(L, Wc);
abs_and_phase = [abs, pha+180]
Lzpk = zpk(minreal(L))
%bode(L)
%nyquist(L)

Rz = c2d(R, Ta, 'tustin');
Lzzpk = zpk(minreal(Rz*Gz))
Rzzpk = zpk(minreal(Rz))
PIstepinfo = stepinfo(tf(L/(1+L)))
%% Regler Design Final alternative Variante
% alternativ: PI ohne Kompensationsterm
[abs, pha] = bode(Gq*1/q, Wc);
Tialt = tan((phi-pha-180)*pi/180)/Wc
Ralt1 = 1/q*(1+q*Tialt);
Lalt1 = Gq*Ralt1
[abs, pha] = bode(Lalt1, Wc);

Vialt = 1/abs;
Ralt = Ralt1 * Vialt;
Lalt = Gq*Ralt;
[abs, pha] = bode(Lalt, Wc);
abs_and_phase_alt = [abs, pha+180]
Laltzpk = zpk(minreal(Lalt))
%bode(Lalt)
%nyquist(Lalt)
%pzmap(Lalt)

Rzalt = c2d(Ralt, Ta, 'tustin');
Lzaltzpk = zpk(minreal(Rzalt*Gz))
Rzaltzpk = zpk(minreal(Rzalt))
PIaltstepinfo = stepinfo(tf(Lalt/(1+Lalt)))
% Moral: Kompensationsregler wegen schlechtem Stabilitätsverhalten und
% nicht-einhalten der Anforderungen
%% Analyse des Systemverhaltens bei Messrauschen
% T^#_n,u
Tqnu = minreal(-R/(1+R*Gq))
subplot(1, 2, 1)
bode(Tqnu);
legend('$T^\#_{n,u}$', 'interpreter', 'latex')
% T^#_n,y
Tqny = minreal(-R*Gq/(1+R*Gq))
subplot(1, 2, 2)
bode(Tqny);
legend('$T^\#_{n,y}$', 'interpreter', 'latex')

save('GSM_2_5.mat')