%----------------------------------------------------------------
%       The Swing Up Control Problem For The Acrobot  
%       LQR control  
%
%       Mark W.Spong
%       IEEE Control System February 1995
%        f1=20 f2=8
%        Call acrobot_eq6.m
%
%            2005.7.28                          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=27.95;  %27%27.9
f2=9;   %9%9
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

%-----------------  Nonlinear 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=6000;  %6000
flag=0;
while(ni <= tend)      
                    
           [Tn,Yn]=ode45(@acrobot_eq6,[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);
           x0=Yn(size(Yn,1),1:4);
           ni=ni+1;
           
           
           %===============================================
           %Link2が360度になったら0[rad]とする
           if x0(1,2)<= -2*pi
               %x0(1,2)=0.0;
               x0(1,2)=x0(1,2) + 2*pi;
           end 
          
           %==================================================検討
           if x0(1,1)>= pi/2-0.05  &&  x0(1,2) >= 0-0.05
               x0(1,1)
               x0(1,2)
               break;
           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);

%==================================================================
%           Linear Controller
%           LQG Controller
%==================== LQR Control==================================
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

d11=m1*lc1^2+m2*(l1^2+lc2^2+2*l1*lc2)+I1+I2;
d22=m2*lc2^2+I2;
d12=m2*(lc2^2+l1*lc2 )+I2;
d21=d12;

II=[-(m1*lc1+m2*l1)*g-m2*lc2*g    -m2*lc2*g
     -m2*lc2*g                    -m2*lc2*g];
 

A=[zeros(2,2)                   eye(2,2)
   -inv([d11 d12;d21 d22])*II   zeros(2,2)];

B=[zeros(2,1)
   inv([d11 d12;d21 d22])*[0 1]'  ];

C=eye(4,4);
D=[0 0 0 0]';

QQ=[1e0  0  0  0
    0  1e0  0  0
    0  0  1e0  0
    0  0  0  1e0];
RR=1;
Fgain= lqr(A,B,QQ,RR)
linear_sys=ss(A-B*Fgain,[],C,[]);

tt=0:sampling:4;
%====================
% Notice!
%  x0(1,1)=q1-pi/2
%====================

x0(1,1)=x0(1,1)-pi/2;
%x0
[yy,tt,xx] = initial(linear_sys,x0,tt);

figure(2)
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);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Swing up Controller + Linear Controller
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TTsize=size(TT,1);
ttsize=size(tt,1);
yysize=size(yy(:,1),1)
yy(:,1)=(pi/2)*ones(yysize,1) + yy(:,1);
ttlq=TTsize*sampling*ones(ttsize,1) + tt;
ttotal=vertcat(TT,ttlq);
ytotal=vertcat(YY,yy);

figure(3)
plot(ttotal,ytotal(:,1),ttotal,ytotal(:,2));
axis([0  8 -8  +3])
xlabel('Time[s]');
ylabel('Angle of Link1&Link2[rad]')
legend('Link1 q1','Link2 q2',2);
title('Swing Up and balancing motion')
str1(1) = {'switch to linear controller'};
text(4,-2,str1)

%----Animation---%
figure(4)
xxa=l1*cos(ytotal(:,1));
yya=l1*sin(ytotal(:,1));
xxxa=l2*cos(ytotal(:,2) + ytotal(:,1));
yyya=l2*sin(ytotal(:,2) + ytotal(:,1));

h=plot([0;xxa(1);(xxa(1)+xxxa(1))],[0;yya(1);(yya(1)+yyya(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(ttotal)
    set(h,'xdata',[0;xxa(i);(xxa(i)+xxxa(i))],'ydata',[0;yya(i);(yya(i)+yyya(i))]);
    drawnow
    pause(0.005);
    grid on
end   


%----Animation avi---%
figure(5)
xxa=l1*cos(ytotal(:,1));
yya=l1*sin(ytotal(:,1));
xxxa=l2*cos(ytotal(:,2) + ytotal(:,1));
yyya=l2*sin(ytotal(:,2) + ytotal(:,1));

mov=avifile('example3.avi');  %change
for i=1 : size(ttotal)
    h1=plot([0;xxa(1);(xxa(1)+xxxa(1))],[0;yya(1);(yya(1)+yyya(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;xxa(i);(xxa(i)+xxxa(i))],'ydata',[0;yya(i);(yya(i)+yyya(i))],'Erasemode','xor','linewidth',2,'Marker','o');
    drawnow
    pause(0.001);
    grid on
    F=getframe(gca);
    mov=addframe(mov,F);
end   
mov=close(mov);
movie2avi(mov)
save('I:\USR\kizawa\reserch\acrobot_spong\example3.avi')

