clc
clear
close all

s = tf('s');
G = (60*(s+10))/(s*(s+2) * (s^2+15*s+100))
G = zpk(G)
G.DisplayFormat = 'Frequency'

kg = 3
%sistema di tipo 0

kd = 0.1
kh = 1/kd

e_ramp = 0.01
kc = (kd^2)/(e_ramp*kg)

e_grad = 0.04
d1 = 0.5
kc1 = d1/(e_grad*kh)

d2 = 0.3
kc2 = d2/(e_grad*kg*kh)
kc = max([kc2 kc1 kc])


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

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

Ts = 0.03
Bw = 3/Ts
wc = 0.75*Bw

%%KC = 1.25 di base
Mr_dB = 1
Mr = 10^(1/20)

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

Ts = 0.2
Bw = 3/Ts
wc = 11 %0.7 * Bw

%proviamo
L = kc*G*kh
[m,f] = bode(L,wc);

m_dB = 20*log10(m)
my_margin = f +180

epsilon = 5
dfm = fm_deg - my_margin + epsilon

%faccio 2 reti anticipatrici
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)

%rete ritardatrice
alpha = 1/m
%alpha = 10^(-m_dB/20)
tau = 100/wc
Cr = (1+tau*alpha*s)/(1+tau*s)

L2 = Cr*L1
%bode(L2)

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

%%Parte 2
% 
% G = (100*(s+10))/(s^2*(s+1)*(s+50))
% %bode(G), margin(G), nyquist(G), roots(W) mi dicono che 
% %solo C proporzionale, NO
% %C = 1
% 
% %C integrale, un alttro polo in 0, è un bordello, rimango sempre sotto i
% %180 di fase, diocane
% %C = (s+0.1)/s
% 
% %C = 10*(s + 0.1)
% C = 1 + s
% L = C*G
% margin(L)
% W=feedback(L,1)
% %step(W)