%----------------------------------------------------------------
%       Phase Plane Trajectory(位相平面軌跡)
%  
%                   g
%     q'' +   ---------- sin(q)=0
%                   l
%     x1:=q   x2:=q'
%     then
%     x'1=x2
%     x'2=-(g/l)sin(x1)
%
%       l=1[m]  g=9.8m/s^2
%       q(0)=2.5[rad]  q'(0)=0[rad/sec]
%
%       Notice!
%       call trajectory1_eq0.m
%
%            2006.3.30                          produced by kizawa
%--------------------------------------------------------------------------

clc
clear all
format long e

global ni;

%----setup---%
sampling=0.001;%sampling time of simulation 
kaisu=1000;
timespan=sampling*kaisu;% simulation time
%--- initial state ----
x1=2.5;%[rad]
x2=0.0;%[rad/s]        

x0=[x1  x2 ]; %initial condition


%---mechanical parameter---
l=1;        %[m]
g = 9.8;    %[m/s^2] acceleration of gravity


%-----------------  Non linear system------------------------------------------------
%---definition---   
%  y(1,1)   y(2,1)        
%           dy(1,1)        
%-------------------------------------------------------------------------------------

%---Y(1,1)     Y(2,1)               
%    x1          x2
%--------------------------------------------------------------------------

disp('It is under calculation now!!')
ni=1;
tend=4000;  %6000   応答時間に合わせて変える
flag=0;
while(ni <= tend) 
    
             [Tn,Yn]=ode45(@trajectory1_eq0,[0 sampling],x0,[],l,g); %変えること
                    
                                                                                                    
           if ni==1
              YY(1,:)=Yn(1,1:2); 
              TT(1,:)=Tn(1,1); 
             % VIN(1,:)=Vin;
             
           else    
             YY(ni,:)=Yn(size(Yn,1),1:2);
             TT(ni,1)=sampling*ni; 
             %VIN(ni,1)=Vin;
         
           end
           %---- Up data------
           size(Yn,1);
           x0=Yn(size(Yn,1),1:2);
           ni=ni+1;
           
           
end 


figure(1)
plot(YY(:,1) ,YY(:,2));
ax1=findobj(gca,'Type','Axes','Visible','on');
set(ax1,'fontsize',12);  %目盛りを大きく
axis([-4  +4 -8  +8])
xlabel('x_1','fontsize',12);
ylabel('x_2','fontsize',12)
title('Phase Plane Trajectory','fontsize',12)
text(2,4,'( x_1(0),x_2(0) )=(2.5,0)' );








