%
% pendolo_lin.m
%
% Fondamenti di Automatica - Vercelli 
% (c) Fabrizio Dabbene - 2004
%
% Simulazione dell'evoluzione libera del pendolo
% Confronto fra il sistema non-lineare (in rosso)
% e sistema linearizzato (in blu)
%

clear all;
close all;
clear global;

% valori numerici
% definisco le variabili globali (per poterle 'vedere' nella funzione f_pendolo)
global m l g beta;

m=1;
l=1;
g=9.8;
% provare diversi valori di smorzamento 
% beta=0;   %non smorzato
 beta=.5;  %poco smorzato
% beta=1;   % molto smorzato:

% condizioni iniziali (provare diversi valori)
x0=[.1;0];    

% tempo di simulazione
t0=0;           % istante iniziale
tf=30;          % istante finale 
dt=0.1;         % intervallo di tempo
t=t0:dt:tf;     % tempo di simulazione
Nt=length(t);   % numero di istanti di tempo



%------------------ simulazione sistema non lineare ------------------
[t,x_nl]=ode23('f_pendolo',t,x0);
y_nl=x_nl(:,1);   % uscita sistema non-lineare (angolo)          

%------------------ simulazione sistema linearizzato ------------------
J=m*l^2;
A=[0 1;-g/l -beta/J];
B=[0;1/J];
C=[1 0];
D=0;
u=zeros(1,Nt); %ingresso nullo
sys_pendolo=ss(A,B,C,D);
[y_lin,t,x_lin]=lsim(sys_pendolo,u,t,x0); 


figure(1)
set(1,'pos',[20 300 970 393]);hold on % position/size of sim. window

%---------------- evoluzione dell'uscita  -----------------------
figure(1),subplot(2,2,2)
plot(t,y_nl(:,1),'r');   %sistema non lineare - in rosso
hold on
plot(t,y_lin,'b');       %sistema linearizzato - in blu
title('evoluzione della uscita')
xlabel('t');
ylabel('x_1');

%----- evoluzione dei movimenti nello spazio degli stati --------
figure(2)
plot3(t,x_nl(:,1),x_nl(:,2),'r');       %sistema non lineare - in rosso
hold on; plot3(t0,x0(1),x0(2),'xr')
plot3(t,x_lin(:,1),x_lin(:,2),'b');     %sistema linearizzato - in blu
grid;
title('evoluzione dei movimenti nello spazio degli stati')
xlabel('t');
ylabel('x_1');
zlabel('x_2');

%------ evoluzione delle traiettorie nello spazio degli stati ----
figure(1);subplot(2,2,4)
plot(x_nl(:,1),x_nl(:,2),'r');          %sistema non lineare - in rosso
hold on; plot(x0(1),x0(2),'xr')
plot(x_lin(:,1),x_lin(:,2),'b');        %sistema linearizzato - in blu
grid;
title('traiettorie nello spazio degli stati');
xlabel('x_1');
ylabel('x_2');

%------ simulazione pendolo
figure(1);subplot(2,2,1)
%------ init
x=x_nl;lcg=1;lb=lcg*2;        % lunghezza della barra (m)
wb=0.016*lb;vgap=0.005*lb;wtr=2.12*lb;htr=0.03*lb;.12*lb;xptr=[wtr/2;wtr/2;-wtr/2;-wtr/2];
yptr=[0;-htr;-htr;0];subplot(1,2,1);ptr=fill(xptr,yptr,'green');hold on
axis([-2.5,2.5,-2.5,2.5]);
% nl
xdb=[wb/2;wb/2;-wb/2;-wb/2];
ydb=[0;-lb;-lb;0];
theta_nl=x0(1); % posizione iniziake
v_nl=[cos(theta_nl) sin(theta_nl);-sin(theta_nl) cos(theta_nl)];
db_nl=[xdb -ydb]*v_nl';
xpb_nl=db_nl(:,1);ypb_nl=-htr-2*vgap-db_nl(:,2);
pb1_nl=fill(xpb_nl,ypb_nl,'red','EraseMode','background'); 
% lin
xdb_lin=[wb/2;wb/2;-wb/2;-wb/2];
ydb_lin=[0;-lb;-lb;0];
theta_lin=x0(1); % posizione iniziake
v_lin=[cos(theta_lin) sin(theta_lin);-sin(theta_lin) cos(theta_lin)];
db_lin=[xdb -ydb]*v_lin';
xpb_lin=db_lin(:,1);ypb_lin=-htr-2*vgap-db_lin(:,2);
pb1_lin=fill(xpb_lin,ypb_lin,'blu','EraseMode','background'); 

ptime=text(-2,2,'00 sec');
%------ end init
pause
%------ simul
for k=2:Nt
tic; while toc<dt; end;
%nl
theta_nl=x_nl(k);
v_nl=[cos(theta_nl) sin(theta_nl);-sin(theta_nl) cos(theta_nl)];
db_nl=[xdb -ydb]*v_nl';
xpb_nl=db_nl(:,1);ypb_nl=-htr-2*vgap-db_nl(:,2);
set(pb1_nl,'Xdata',xpb_nl,'Ydata',ypb_nl);
drawnow;
%lin
theta_lin=x_lin(k);
v_lin=[cos(theta_lin) sin(theta_lin);-sin(theta_lin) cos(theta_lin)];
db_lin=[xdb -ydb]*v_lin';
xpb_lin=db_lin(:,1);ypb_lin=-htr-2*vgap-db_lin(:,2);
set(pb1_lin,'Xdata',xpb_lin,'Ydata',ypb_lin);
%
tt=strcat(int2str(t(k)),' sec');
set(ptime,'String',tt);
drawnow;
end
%------end simul