% Esercitazioni di Controllo Digitale A.A. 2002/2003
% Titolare del corso Prof. A. Bemporad
% Esercitazioni tenute da Ing. Nicolo' Giorgetti
%
% 2 masse accoppiate da una molla e uno smorzatore, una forza spinge la prima e 
% si misura la posizione della seconda massa.
%
% Argomenti introdotti: + pole placement
%                       + controllo LQR
%                       + simulazione sistema ad anello chiuso

% Parametri del modello
k=1;
beta=2;
m1=10;
m2=10;

% Rappresentazione spazio di stato
A=[0 1 0 0;...
      -k/m1 -beta/m1 k/m1 beta/m1;...
      0 0 0 1;
      k/m1 beta/m2 -k/m2 -beta/m2];
b=[0 1/m1 0 0]';
c=[0 0 1 0];
d=[0];
sys_2m=ss(A,b,c,d)
pause;

% Versione tempo-discreta del sistema
Ts=5;
sys_td_2m=c2d(sys_2m, Ts)
pause;


% Luogo delle radici del sistema delle 2 molle
P=eig(sys_td_2m.a)
pause;

% Si puo' vedere anche la posizione dei poli e degli zeri
figure('Name','Poli anello aperto');
pzmap(sys_td_2m)
pause;

% Poli ad anello chiuso all'interno del cerchio di raggio unitario
PP=[P(1:2,:); 0.5; -0.5]

K=-place(sys_td_2m.a,sys_td_2m.b,PP)
Pplace=eig(sys_td_2m.a+sys_td_2m.b*K)
pause;

% Simulazione del sistema controllato
x0=[10 0 11 0]';
hz=20;

T=1:hz;
T=Ts*(T-1);

% Plot del controllo con pole-placement
UU=[];
XX=[];
xi=x0;
for i=1:hz
   XX=[XX xi];
   UU=[UU K*xi];
   xi=(sys_td_2m.a+sys_td_2m.b*K)*xi;
end

figure('Name','Pole placement');
subplot(2,1,1);
plot(T,UU);
title('Controllo con pole placement');
xlabel('T [s]');
ylabel('[N]');


subplot(2,1,2);
plot(T,XX);
legend('s1','v1','s2','v2');
title('Regolazione dello stato con pole placement');
xlabel('T [s]');
ylabel('[m] o [m/s]');



% Calcolo della retroazione ottima LQR
Q=eye(4);
R=5;       % provare a porre R=5

[KLQ, P, E]=dlqr(sys_td_2m.a, sys_td_2m.b,Q,R);
KLQ=-KLQ
pause;

% Poli ad anello chiuso del sistema controllato in modo ottimo
E

sys_cl_lq=ss(sys_td_2m.a+sys_td_2m.b*KLQ,zeros(size(sys_td_2m.a,1),1),sys_td_2m.c, sys_td_2m.d);

figure('Name', 'Confronto poli anello aperto/LQR');
pzmap(sys_td_2m, sys_cl_lq)
pause;

% Plot del controllo LQR
UU=[];
XX=[];
xi=x0;
for i=1:hz
   XX=[XX xi];
   UU=[UU KLQ*xi];
   xi=sys_cl_lq.a*xi;
end


figure('Name','Controllo LQR');
subplot(2,1,1);
plot(T,UU);
title('Controllo Ottimo');
xlabel('T [s]');
ylabel('[N]');


subplot(2,1,2);
plot(T,XX);
legend('s1','v1','s2','v2');
title('Regolazione dello stato con controllo ottimo');
xlabel('T [s]');
ylabel('[m] o [m/s]');

   
   
