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]);
info.RiseTime;

fprintf("Picco di risonanza del sistema: %.3f",peak)