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])

kc = 1

L = kc * G * kh

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.8*Bw % 50<-->80

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

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

% %%derivatrice con carta normalizzata
m = 10;
wt = 2;
tau = wt / wc

Ca = (1 + tau * s)/(1 + (tau/m)*s)
% 
L1 = Ca*L

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

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