clc
clear
close all


s = tf('s')
kd = 10
kh = 1/kd

G = (s+5)/(s*(s^2+100+10*s)*(s+1))
G = zpk(G)
G.DisplayFormat = 'Frequency'

d1 = 0.5
e_ramp = 1
e_grad = 0.1

kd = 10
kh = 1/kd
kg = 0.05

kc = (kd^2)/(e_ramp * kg)
kc1 = (d1)/(e_grad * kh)
kc = max([kc kc1])

kc = kc + 2000

Mr_dB = 2
Mr = 10^(Mr_dB/20)

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

Ts = 0.1
Bw = 3/Ts

wc = 0.8 * Bw

%provamm
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

%reti anticipatrici sicuro
wt = 5.9
m = 16
alpha = 1/m
tau = wt/wc
%dfm = dfm/2
%alpha = (1-sind(dfm))/(1+sind(dfm))
%tau = 1/(sqrt(alpha)*wc)

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

W = feedback(L1,1)
bode(W)


%parte 2
% %C = (2500*(s+1))/s
% C=(5915.6521*(s+0.01))/s
% L = C*G*kh
% 
% margin(L)
% 
% W = feedback(L,1)
% step(W)