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';

G
kg = 0.5
kd = 0.01
kh = 1/kd

e_ramp = 0.001
e_grad = 0.002
d1 = 2

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

kc = max([kc kc1])

L = kc * G * kh
bode(L)

Mr_dB = 3;
Ts = 0.03

Mr = 10^(Mr_dB/20);

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

Bw = 3 / Ts
wc = 0.70*Bw % 50<-->80

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

epsilon = 20
dfm = fm_deg - (180+f) + epsilon

%%devo perdere 18dB di modulo e guadagnare 33deg di fase
%derivatrice

alpha = (1-sind(dfm))/(1+sind(dfm))
tau = 1/(wc * sqrt(alpha))

Ca = (1+tau*s)/(1+alpha*tau*s)

L1 = Ca * L
[m,a] = bode(L1,wc)

%%perdo 22dB di modulo con rete ritardatrice
%%integratice

alpha = 1/m
tau = 100/wc

Cr = (1+tau*alpha*s)/(1+tau*s)
L2 = Cr * L1
[m,f] = bode(L2,wc)

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

C = kc * Cr * Ca
C = zpk(C)
C.DisplayFormat = 'Frequency'
% [m,f] = bode(L1,wc)
% % %bode(L1)
% m_dB = 20*log10(m)
% % 
% 
% W = feedback(L1,1)
% bode(W)