clc, clear all, close all
% F.d.t. Processo
wn=pi; csi=1/3;
P=tf(0.01,conv([1/wn^2 2*csi/wn 1],[1/10 1]))

figure(1)   
    margin(P), grid

Kp_cr=10^(48.9/20); tau_cr=2*pi/5.5;

% Regolatore tipo P:
Kp_p=0.5*Kp_cr; 
Cp=Kp_p

Wp=feedback(Cp*P,1);

% Regolatore tipo PI:Kcr= 4*1/(pi*0.004806)=265
I=tf(1,[1 0]);
Kp_pi=0.45*Kp_cr; Ti_pi=0.8*tau_cr;
Cpi=Kp_pi*(1+I/Ti_pi);

Wpi=feedback(Cpi*P,1);

% Regolatore tipo PID:

D=tf([1 0],1);
Kp_pid=0.6*Kp_cr; Ti_pid=0.5*tau_cr;
Td_pid=0.125*tau_cr;
Cpid=Kp_pid*(1+I/Ti_pid+D*Td_pid)

Wpid=feedback(Cpid*P,1);

% Regolatore tipo PID Reale:
N=10;
Cpid_real=Kp_pid*(1+I/Ti_pid+tf([Td_pid,0],[Td_pid/N 1]))

Wpid_real=feedback(Cpid_real*P,1);

%%
figure(2)
    step(Wp,Wpi,Wpid,P), grid
    legend('W_P(s)','W_{PI}(s)','W_{PID}(s)','P(s)')
figure(3)
    step(Wpid,Wpid_real), grid
    legend('W_{PID}(s)','W_{PID}^{Reale}(s)')
figure(4)
    bode(Cp*P,Cpi*P,Cpid*P,Cpid_real*P,P), grid
    legend('C_P(s)P(s)','C_{PI}(s)P(s)','C_{PID}(s)P(s)','C_{PID}^{Reale}(s)P(s)','P(s)')
figure(5)
    nichols(Cp*P,Cpi*P,Cpid*P,P), grid
    legend('C_P(s)P(s)','C_{PI}(s)P(s)','C_{PID}(s)P(s)','P(s)')
    xlim([-270 0])
figure(6)
    bode(Wp,Wpi,Wpid,Wpid_real), grid
    legend('W_P(s)','W_{PI}(s)','W_{PID}(s)','W_{PID}^{Reale}(s)')

%%
% Regolatore tipo PID some overshoot:

Kp_pid_some_s=0.33*Kp_cr; 
Ti_pid_some_s=0.5*tau_cr;
Td_pid_some_s=0.33*tau_cr;
Cpid_some_s=Kp_pid_some_s*(1+I/Ti_pid_some_s+D*Td_pid_some_s)

Wpid_some_s=feedback(Cpid_some_s*P,1);
% Regolatore tipo PID no overshoot:

Kp_pid_no_s=0.22*Kp_cr; 
Ti_pid_no_s=1*tau_cr;
Td_pid_no_s=0.33*tau_cr;
Cpid_no_s=Kp_pid_no_s*(1+I/Ti_pid_no_s+D*Td_pid_no_s)

Wpid_no_s=feedback(Cpid_no_s*P,1);


figure(7)    
    step(Wpid,Wpid_some_s,Wpid_no_s), grid
    legend('W_{PID}','W_{PID some S}','W_{PID no S}')

%% Analisi reiezione al disturbo tra controllore e processo
Wd=feedback(P,Cpid)
Wd_some_s=feedback(P,Cpid_some_s)
Wd_no_s=feedback(P,Cpid_no_s)

Wd_p=feedback(P,Cp)

Wd_pi=feedback(P,Cpi)


figure(8)    
    step(Wd,Wd_some_s,Wd_no_s), grid
    legend('W_{d,PID}','W_{d,PID some S}','W_{d,PID no S}')
