clc
clear 
close all

s = tf('s');
G = (100*(s+500))/((s+10)*(s+20)*(s+400))
G = zpk(G)
G.DisplayFormat = 'Frequency'


kg = 0.625
kd = 0.2
kh = 1/kd
d1 = 0.2
e_ramp = 10^-3
e_dist = 0.3

kc = (kd^2)/(e_ramp * kg)
%kc1 = (d1)/(e_dist*kh) è nullo, controllore di tipo 1

% Mr_dB = 3
% Mr = 10^(Mr_dB/20)
% fm = (2.3-Mr)/1.25
% fm_deg = rad2deg(fm)
% 
% wc = 21 %tra 19.5 e 26
% 
% %proviamo
% C = kc/s
% L = C * G * kh
% 
% [m,f] = bode(L,wc)
% m_dB = 20*log10(m)
% my_margin = f + 180
% 
% epsilon = 10
% dfm = -my_margin + fm_deg + epsilon
% 
% %2 anticipatrici e 1 ritardatrice
% %anticipatrice
% 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)
% 
% %ritardatrice
% alpha = 1/m
% tau = 100/wc
% Cr = (1+tau*alpha*s)/(1+tau*s)
% 
% L2 = Cr*L1
% %bode(L2)
% 
% W = feedback(L2,1)
% bode(W)
% bandwidth(W)


C = (kc*(s+0.1))/s
L = C * G * kh
margin(L)
W = feedback(L,1)