clc
clear
close all

s = tf('s');
G = (800*(s+25))/(s*(1+0.02*s)^2 * (s+200)^2)
G = zpk(G)
G.DisplayFormat = 'Frequency'

kg = 0.5
%sistema di tipo 0

kd = 0.01
kh = 1/kd

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

e_grad = 0.002
d1 = 2
kc1 = d1/(e_grad*kh)

kc = max([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

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

epsilon = 20
dfm = fm_deg - my_margin + epsilon

%devo guadagnare 30gradi e perdere 18dB di modulo
%rete anticipatrice
alpha = (1-sind(dfm))/(1+sind(dfm))
tau = 1/(wc*sqrt(alpha))
Ca = (1+tau*s)/(1+alpha*tau*s)

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

alpha = 1/m
tau = 100/wc

Cr = (1+tau*s*alpha)/(1+tau*s)
L2 = Cr * L1
%bode(L2)

W = feedback(L2,1)
bode(W)
peak = getPeakGain(W);
peak_dB = 20*log10(peak)
bandwidth(W)