clear all; %cancella tutte le variabili nel workspace di MATLAB
close all; %chiude tutti grafici aperti da MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% parametri fisici del problema
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

m1 = 1.2;  % massa pendolo 1 [kg]
m2 = 0.3;  % massa pendolo 2 [kg]
l1 = 1.3;  % lunghezza asta pendolo 1 [m]
l2 = 0.5;  % lunghezza asta pendolo 1 [m]
b = 4;     % attrito viscoso [N.m/(rad/s)]
g = 9.8;   % costante gravitazionale [m/s^2]
k = 7;     % costante molla torsionale [N.m/rad]

J1 = m1*l1^2; % momento inerzia pendolo 1 [kg.m^2]
J2 = m2*l2^2; % momento inerzia pendolo 2 [kg.m^2]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% punti di lavoro per posizione verticale dei pendoli
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

theta1_0 = pi;
theta2_0 = pi;
u0 = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% funzioni di trasferimento sistema linearizzato
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% % primo metodo di calcolo delle funzioni di trasferimento
% T1 = tf([J1 b k-m1*l1*g],[1]);
% T2 = tf([J2 b k-m2*l2*g],[1]);
% T3 = tf([-b^2 -2*k*b -k^2],[1]); 
% T4 = tf([b k],[1]);
% 
% P1_b = minreal(T2/(T1*T2 + T3));   % funzione di trasferimento. INPUT=u OUTPUT=theta1
% P2_b = minreal(T4/(T1*T2 + T3));   % funzione di trasferimento. INPUT=u OUTPUT=theta2

% secondo metodo di calcolo delle funzioni di trasferimento
num1 = [J2 b k-l2*m2*g];
den1 = [J1*J2 b*(J1+J2) k*(J1+J2)-g*(m1*l1*J2+m2*l2*J1) -b*g*(m1*l1+m2*l2) m1*m2*l1*l2*g^2-k*g*(m1*l1+m2*l2)];
num2 = [b k];
den2 = den1;

P1 = minreal(tf(num1,den1));
P2 = minreal(tf(num2,den2));
[theta_12,t12]=step(P2,2);    % risposta a gradino unitario in catena aperta da t=0 a t=2
figure(1);
plot(t12,theta_12*180/pi+180,'k');  % grafico colore nero in gradi 
xlabel('tempo t [sec]'); 
ylabel('\theta_2 [gradi]'); 
title('Risposta a ingresso u a gradino unitario');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% progettazione compensatori
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

numC1 = [25 50];  % controllo PD
denC1 = [0.001 1]; % aggiungo un polo molto negativo per rendere la funzione di trasferimento C1 propria
C1 = minreal(tf(numC1,denC1));

Pc1 = minreal(feedback(1,P1*C1));
[theta_1,t1]=impulse(Pc1);    % verifica al disturbo di tipo impulsivo
theta_1=theta_1*180/pi;   %conversione radianti --> gradi
figure(2);
plot(t1,theta_1,'b');  % grafico colore blue.
xlabel('tempo t [sec]'); 
ylabel('\theta_1 [gradi]'); 
title('Risposta a disturbo impulsivo unitario');
hold on;   % mantiene tutti i grafici nella figura

Pc12 = minreal(P2*Pc1);  % sistema a catena chiusa dopo inserimento controllore C1
[theta_2c,t2c]=step(Pc12,5);    % verifica al disturbo di tipo impulsivo
theta_2c=theta_2c*180/pi;   %conversione radianti --> gradi
figure(3);
plot(t2c,180+theta_2c,'b');  % grafico colore blue.
xlabel('tempo t [sec]'); 
ylabel('\theta_2 [gradi]'); 
title('Risposta a disturbo a gradino unitario dopo inserimento C_1');

numC2 = 108*[1 1];   % controllo PI
denC2= [1 0];
C2 = tf(numC2,denC2); 

Pc=minreal(feedback(Pc12*C2,1));   %sistema a catena chiusa dopo inserimento controllore C2
[theta_2,t2]=step(Pc);
figure(4);
theta_2=10*theta_2*(pi/180);  %risposta a gradino di 10 gradi, utilizzando la proprieta' di linearita'
plot(t2,theta_2*180/pi,'r');  %grafico colore rosso
xlabel('tempo t [sec]'); 
ylabel('\theta_2 [gradi]'); 
title('Risposta a riferimento a gradino unitario');

d0 = 10;  %disturbo in ingresso in Nm
d1 = 10;  %ampiezza della sinusoide di disturbo
r = 10; %riferimento desiderato theta2 in gradi