%----------------------------------------------------------------
%       The Swing Up Control Problem For The Acrobot  
%  
%       Mark W.Spong
%       IEEE Control System February 1995
%        f1=16 f2=6
%        Call acrobot_eq4.m
%
%            2005.5.16                          programed 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 ----
th1=-pi/2; %angle of rink 1(rad)  -3.14/2:真下(水平が基準)
th2=0.00; %angle of rink 2(rad)           
d_th1=0.0; %angle velocity of rink1(rad/s)
d_th2=0.0; %angle velocity of rink2(rad/s)

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

%---mechanical parameter---
f1=16;
f2=6;
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---
%第１リンク角度　第２リンク角度　第１リンク角速度  第２リンク角速度     第１リンク角加速度　第２リンク角加速度   
%---y(1,1)      y(2,1)         y(3,1)          y(4,1)
%
%                             dy(1,1)          dy(2,1)               dy(3,1)          dy(4,1)
%-------------------------------------------------------------------------------------

%---Y(1,1)     Y(2,1)    Y(3,1)       Y(4,1)      
%第１リンク角度　第２リンク角度　第１リンク角速度  第２リンク角速度     
%-------------------------------------------------------------------------------------


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%-------------------------
disp('It is under calculation now!!')
ni=1;
tend=10000;  %10000
flag=0;
while(ni <= tend)
           
               
           [Tn,Yn]=ode45(@acrobot_eq4,[0 sampling],x0,[],v,m1,m2,l1,l2,lc1,lc2,I1,I2,g,f1,f2);
           
           
           
           if ni==1
              YY(1,:)=Yn(1,1:4); 
              TT(1,:)=Tn(1,1); 
             % VIN(1,:)=Vin;
             
           else    
             YY(ni,:)=Yn(size(Yn,1),1:4);
             TT(ni,1)=sampling*ni; 
             %VIN(ni,1)=Vin;
         
           end
           %---- Up data------
           size(Yn,1);
           if flag==0
              x0=Yn(size(Yn,1),1:4);
           end  
         
           ni=ni+1;
           
           %===============================================
           %Link2が360度になったら0[rad]とする
           if x0(1,2)<= -2*pi
               x0(1,2)=0.0;
           end 
          
           %==================================================
end 
ni

figure(1)
plot(TT,YY(:,1),TT,YY(:,2));
axis([0  sampling*ni -8  +8])
xlabel('Time[s]');
ylabel('Angle of Link1&Link2[rad]')
legend('Link1 q1','Link2 q2',2);

%----Animation---%
figure(2)
xx=l1*cos(YY(:,1));
yy=l1*sin(YY(:,1));
xxx=l2*cos(YY(:,2) + YY(:,1));
yyy=l2*sin(YY(:,2) + YY(:,1));

h=plot([0;xx(1);(xx(1)+xxx(1))],[0;yy(1);(yy(1)+yyy(1))],'Erasemode','xor','linewidth',2,'Marker','o');
axis([-l1-l2-0.1 l1+l2+0.1 -l1-l2-0.1 l1+l2+0.1]);
for i=1 : size(TT)
    set(h,'xdata',[0;xx(i);(xx(i)+xxx(i))],'ydata',[0;yy(i);(yy(i)+yyy(i))]);
    drawnow
    pause(0.005);
    grid on
end   

%----Animation avi---%
figure(3)
xx=l1*cos(YY(:,1));
yy=l1*sin(YY(:,1));
xxx=l2*cos(YY(:,2) + YY(:,1));
yyy=l2*sin(YY(:,2) + YY(:,1));
mov=avifile('example1.avi');
for i=1 : size(TT)
    h1=plot([0;xx(1);(xx(1)+xxx(1))],[0;yy(1);(yy(1)+yyy(1))]);
    set(gca,'xlim',[-l1-l2-0.1 , l1+l2+0.1],'ylim',[-l1-l2-0.1 , l1+l2+0.1]);
    set(h1,'xdata',[0;xx(i);(xx(i)+xxx(i))],'ydata',[0;yy(i);(yy(i)+yyy(i))],'Erasemode','xor','linewidth',2,'Marker','o');
    drawnow
    pause(0.005);
    grid on
    F=getframe(gca);
    mov=addframe(mov,F);
end   
mov=close(mov);
movie2avi(mov)
save('I:\USR\kizawa\reserch\acrobot_spong\example1.avi')





