% Script to analyse the pendulum system.
% Run the nonlinear simulation (pendulum.mdl) using Simulink before executing this script.
% The first plot is for a reference solution with
% Theta=0, and Thetadot=0.
% The second plot is for a reference solution with
% Theta=0, and Thetadot not=0. This better matched the damping.


disp(' ');disp('Start here');disp(' ');
disp('Find trim condition')
[X,U,Y,DX]=trim('pendulum')
disp('Reference point about which to linearize')
xR=[0;0]
[a,b,c,d]=linmod('pendulum',xR,[]) %Find linear model of nonlinear pendulum
b=[0;0]
c=[1,0;0,1]
d=[0;0]
[Wn,Z]=damp(a) % Find properties of the poles of linearized system
period=2*pi/Wn(1)
% Periods in 10 second
disp('Periods in 48 seconds')
P48=48/period
PSYS=ss(a,b,c,d) %Create a linear state space system
disp('Initial condition for linear simulation')
x0=[.5;0] %Use this initial condition for linear simulation
Tfinal=max(tnlsim);
t=0:.1:Tfinal';
u=zeros(size(t));
[y,t]=lsim(PSYS,u,t,x0); %Do linear simulation
%Plot the linear results and the nonlinear results
%  which have been stored in tnlsim and ynlsim.
figure(1) 
str1=['YOUR NAME: For Pendulum linearized about theta= ',num2str(xR(1)),', thetadot= ',num2str(xR(2))];
str2=[', Wn= ',num2str(Wn(1)),', Zeta= ',num2str(Z(1))];
subplot(211)
plot(t,y(:,1),'-',tnlsim,ynlsim(:,1),'x')
title([str1,str2])
xlabel('time (sec)')
ylabel('theta (rad)')
legend('linear sim','nonlinear sim')
subplot(212)
plot(t,y(:,2),'-',tnlsim,ynlsim(:,2),'x')
xlabel('time (sec)')
ylabel('thetaDot (r/s)')
legend('linear sim','nonlinear sim')

% Do everything over again at another linearization point 
disp(' ');disp('New reference point');disp(' ');
xR=[0;1]
% xR=[0;.59]  % This seems to give the best match between the time response of the linear system to the nonlinear system.
[a,b,c,d]=linmod('pendulum',xR,[])
b=[0;0];
c=[1,0;0,1];
d=[0;0];
[Wn,Z]=damp(a)
PSYS=ss(a,b,c,d);
disp('Initial condition for linear simulation')
x0=[.5;0] %Use this initial condition for linear simulation
t=0:.1:Tfinal';
u=zeros(size(t));
[y,t]=lsim(PSYS,u,t,x0);
figure(2)
str1=['YOUR NAME: For Pendulum linearized about theta= ',num2str(xR(1)),', thetadot= ',num2str(xR(2))];
str2=[', Wn= ',num2str(Wn(1)),', Zeta= ',num2str(Z(1))];
subplot(211)
plot(t,y(:,1),'-',tnlsim,ynlsim(:,1),'x')
title([str1,str2])
xlabel('time (sec)')
ylabel('theta (rad)')
legend('linear sim','nonlinear sim')
subplot(212)
plot(t,y(:,2),'-',tnlsim,ynlsim(:,2),'x')
xlabel('time (sec)')
ylabel('thetaDot (r/s)')
legend('linear sim','nonlinear sim')
