%----------------------------------------------------------------
%       The Swing Up Control Problem For The Acrobot  
%  
%       Mark W.Spong
%       IEEE Control System February 1995
%        f1=16 f2=6
%        Call acrobot_eq1.m
%
%            2006.4.4                          programed by kizawa
%--------------------------------------------------------------------------

clc
clear all
format long e
global ni

%----setup---%
sampling=0.01;%sampling time of simulation 
%kaisu=1000;
%timespan=sampling*kaisu;% simulation time
%--- initial state ----
th2=-pi/2; %angle of rink 2(rad)  -3.14/2:�^‰º
d_th2=0.00; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
%manipulated variable
v=0.0;%%%%%%%%%

%---mechanical parameter---
m1 = 1;    
m2 = 1;   
l1 = 1;    
l2 = 2;    
lc1= 0.5;  
lc2= 1;   
I1=0.083;%m1*l1^2/12;
I2=0.33;%m2*l2^2/12;
g = 9.8;       %[m/s^2] acceleration of gravity

%-----------------  Non linear system------------------------------------------------
%---definition---
%‘æ‚QƒŠƒ“ƒNŠp“x�@‘æ‚QƒŠƒ“ƒNŠp‘¬“x  
%y(1,1)      y(2,1)        
%
%            dy(1,1)          
%-------------------------------------------------------------------------------------

%---Y(1,1)       Y(2,1)        
%‘æ‚QƒŠƒ“ƒNŠp“x   ‘æ‚QƒŠƒ“ƒNŠp‘¬“x     
%-------------------------------------------------------------------------------------

disp('It is under calculation now!!--->No.1')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
text(-2,6,'\rightarrow','Rotation',-30)
text(-4.5,6,'\rightarrow','Rotation',30)
text(-4,-6,'\leftarrow','Rotation',-30)
text(-2.5,-7,'\leftarrow','Rotation',30)
hold on
%------------------2--------------------------------------------------------
%--- initial state ----
th2=pi/2; %angle of rink 2(rad) 
d_th2=0.0; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.2')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
text(4,6,'\rightarrow','Rotation',-30)
text(2,6,'\rightarrow','Rotation',30)
text(4,-5,'\leftarrow','Rotation',30)
text(2,-5,'\leftarrow','Rotation',-30)
hold on

%------------------3--------------------------------------------------------
%--- initial state ----
th2=-6.5; %angle of rink 2(rad)  
d_th2=10; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.3')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
text(-6.5,12,'\rightarrow','Rotation',30)
text(0,12,'\rightarrow','Rotation',0)
text(6.5,12,'\rightarrow','Rotation',-30)
hold on
%------------------4--------------------------------------------------------
%--- initial state ----
th2=6.5; %angle of rink 2(rad)  
d_th2=-10; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.4')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
text(6.5,-12,'\leftarrow','Rotation',30)
text(0,-12,'\leftarrow','Rotation',0)
text(-6.5,-12,'\leftarrow','Rotation',-30)
hold on
%------------------5--------------------------------------------------------
%--- initial state ----
th2=-6.5; %angle of rink 2(rad)  
d_th2=5; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.5')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
hold on
%------------------6--------------------------------------------------------
%--- initial state ----
th2=6.5; %angle of rink 2(rad)  
d_th2=-5; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.6')
ni=1;
tend=500;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
hold on
%------------------7--------------------------------------------------------
%--- initial state ----
th2=-6.5; %angle of rink 2(rad)  
d_th2=0.5; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.7')
ni=1;
tend=800;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
hold on
%------------------8--------------------------------------------------------
%--- initial state ----
th2=6.5; %angle of rink 2(rad)  
d_th2=-0.5; %angle velocity of rink2(rad/s)          

x0=[th2  d_th2]; %initial condition
disp('It is under calculation now!!--->No.8')
ni=1;
tend=800;  %6000
flag=0;
while(ni <= tend)               
           [Tn,Yn]=ode45(@acrobot_phase1_eq1,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,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);
           if flag==0
              x0=Yn(size(Yn,1),1:2);
           end  
         
           ni=ni+1;
end 
ni

figure(1)
plot(YY(:,1),YY(:,2));
axis([-7  +7 -50  +50])
xlabel('y_1[rad]');
ylabel('y_2[rad/s]')
title('Phase Plane Trajectory','fontsize',12)
hold on
