%Fila 1, es n.1

clear 
close all
clc

s = tf('s');

G = (250*(s+100))/(s*(s+5)*(s+50)^2);

G = zpk(G);
G.DisplayFormat = 'Frequency';

kd = 0.1
kh = 1/kd
kg = 2
e_ramp = 2*10^-3
e_grad = 0.04
d1 = 0.8
kc = (kd^2)/(kg*e_ramp)
kc1 = (d1*kg)/(kg*kh*e_grad)

kc = max([kc kc1])

Mr_dB = 1
Mr = 10^(Mr_dB/20)
Ts = 0.1

fm = (2.3-Mr)/1.25
fm_deg = rad2deg(fm)

Bw = 3/Ts
wc = 20 %0.7*Bw   15--24

%vediamo in che situazione siamo ad anello aperto
L = kc*G*kh
[m,f] = bode(L,wc)
m_dB = 20*log10(m)
my_margin = 180+f

epsilon = 3
dfm = fm_deg - my_margin + epsilon

%devo guadagnare più di 70deg e 5dB di modulo.
%mi aspetto che le reti anticipatrici mi alzino di molto il modulo, dunque
%mi aspetto la necessità di una rete ritardatrice
disp('nuovo dfm')
dfm = dfm/2

alpha = (1-sind(dfm))/(1+sind(dfm))
tau = 1/(wc*sqrt(alpha))

Ca = (1+tau*s)/(1+tau*alpha*s)

L1 = Ca*Ca*L
[m,f]=bode(L1,wc)
m_dB = 20*log10(m)
my_margin = 180+f

%devo perdere 6.86 dB o 2.2 in scala lin

%rete ritardatrice
alpha = 1/m
tau = 100/wc

Cr = (1+tau*alpha*s)/(1+tau*s)

L2 = Cr*L1
[m,f]=bode(L2,wc)
m_dB = 20*log10(m)
my_margin = 180+f

%nichols(L2)
W = feedback(L2,1)
%bode(W)
%figure(2)
%step(W)

peak =getPeakGain(W);
peak_dB= 20*log10(peak);
info = stepinfo(W,"RiseTimeLimits",[0.0 1]);


fprintf("Picco di risonanza del sistema: %.3f dB\n",peak_dB);
fprintf("Tempo di salita del sistema : %.3f sec\n",info.RiseTime);

C = zpk(kc * Cr * Ca * Ca)



% %Fila 2 
% 
% clc
% clear
% close all
% 
% s = tf('s');
% G = (2*(s+1))/((2*s+1)*(8*s+1)*(100*s+1))
% 
% kg = 2
% %sistema di tipo 0
% 
% kd = 5
% kh = 1/kd
% 
% e_ramp = 0.5
% kc = (kd^2)/(e_ramp*kg)
% 
% Mr_dB = 2
% Mr = 10^(Mr_dB/20)
% 
% fm = (2.3-Mr)/1.25
% fm_deg = rad2deg(fm)
% 
% Ts = 25
% Bw = 3/25
% 
% wc = 0.08 %0.65*Bw %0.06 -- 0.096
% 
% C = kc/s
% 
% L = C * kh * G
% [m,f]=bode(L,wc)
% m_dB = 20*log10(m)
% my_margin = f + 180
% 
% eps = 25
% dfm = fm_deg - my_margin + eps
% 
% %avrò bisogno di due derivative e 2 aticipatrici
% dfm = dfm/2
% 
% alpha = (1-sind(dfm))/(1+sind(dfm))
% tau = 1/(wc*sqrt(alpha))
% 
% Ca = (1+tau*s)/(1+tau*alpha*s)
% L1 = Ca*Ca*L
% 
% [m,f]=bode(L1,wc)
% m_dB = 20*log10(m)
% my_margin = f + 180
% 
% %devo perdere 38 db...
% m_dB = m_dB / 2
% alpha = 10^(-m_dB/20)
% tau = 100/wc
% 
% Cr = (1+tau*alpha*s)/(1+tau*s)
% L2 = Cr*Cr*L1
% 
% W = feedback(L2,1);
% bode(W)