%
% pendolo_lyap.m
%
% Fondamenti di Automatica - Vercelli 
% (c) Fabrizio Dabbene - 2004
%
% Simulazione dell'evoluzione libera del pendolo
% Visualizzazione delle funzioni di Lyapunov
%

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=.1;  %poco smorzato
beta=.5;   % molto smorzato:

figure(1)
x1=linspace(-10,10,100);
x2=linspace(-10,10,100);
[X1,X2]=meshgrid(x1,x2);
V=m*g*l*(1-cos(X1)) +1/2*m*l^2*X2.^2;
mesh(X1,X2,V);
xlabel('x_1');
ylabel('x_2');
zlabel('V(x)');
title('Lyapunov function V(x)=mgl(1-cos x_1)+1/2 ml^2 x_2^2')
colormap cool 
shading flat

figure(2)
x1=linspace(-pi,pi,100);
x2=linspace(-10,10,100);
[X1,X2]=meshgrid(x1,x2);
V=m*g*l*(1-cos(X1)) +1/2*m*l^2*X2.^2;
mesh(X1,X2,V);
xlabel('x_1');
ylabel('x_2');
zlabel('V(x)');
title('Lyapunov function V(x)=mgl(1-cos x_1)+1/2 ml^2 x_2^2')
colormap cool 
shading flat

figure(3)
x1=linspace(-10,10,100);
x2=linspace(-10,10,100);
[X1,X2]=meshgrid(x1,x2);
V=m*g*l*(1-cos(X1)) +1/2*m*l^2*X2.^2;
contour(X1,X2,V,20,'c');
xlabel('x_1');
ylabel('x_2');
title('Lyapunov function V(x)=mgl(1-cos x_1)+1/2 ml^2 x_2^2')
hold on
v=m*g*l*(1-cos(3));
contour(X1,X2,V,[v,v],'r');


N=100

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

for k=1:N
figure(3)
hold on
% condizioni iniziali 
[x0(1),x0(2)]=ginput(1);
plot(x0(1),x0(2),'xr')
%------------------ simulazione sistema non lineare ------------------
[t,x_nl]=ode23('f_pendolo',t,x0);
y_nl=x_nl(:,1);   % uscita sistema non-lineare (angolo)          
%----- evoluzione dei movimenti nello spazio degli stati --------
plot(x_nl(:,1),x_nl(:,2),'r');       %sistema non lineare - in rosso

figure(2)
hold on
V=m*g*l*(1-cos(x_nl(:,1))) +1/2*m*l^2*x_nl(:,2).^2;
plot3(x_nl(:,1),x_nl(:,2),V,'r');
plot3(x_nl(:,1),x_nl(:,2),zeros(1,Nt),'b');
end