%%esercizio 1_riprova
clc
clear all
close all

s = tf('s');

G = (s+20)/(s*(s+3)*(s^2+1.5*s+1))
G = zpk(G)
G.DisplayFormat = 'Frequency'

kg = 20/3
kd = 7
kh = 1/kd

e_ramp = 0.1
e_grad = 0.4
d1 = 0.8

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

kc = max([kc kc1])


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

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

Ts = 2.5
Bw = 3 / Ts

wc = 0.6666 * Bw %tra 0.6 e 0.96 rad/sec

L = kc * kh * G
[m,f] = bode(L,wc)
m_dB = 20*log10(m)
my_margin = f + 180

epsilon = 45
dfm = fm_deg - my_margin + epsilon

%dfm = dfm/2

%%rete derivatrice che mi guadagna 60
alpha = (1-sind(dfm))/(1+sind(dfm))
tau = 1/(wc*sqrt(alpha))
Ca = (1 + tau*s)/(1+ tau*alpha*s)

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

%%due reti anticipatrici che in totale perdano 45dB

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
[m,f] = bode(L2,wc)
m_dB = 20*log10(m)
my_margin = f + 180

W = feedback(L2,1)