%----------------------------------------------------------------
%       Acrobot System    
%  
%       自由振動シミュレータ
%       (ma^2+I)q''+cq' + ma sin(q)=0  
%                      c                          mag
%     q'' +        --------   cq' +          ---------- sin(q)=0
%                  (ma^2+I)                   (ma^2+I)
%       Motorを無視
%       Link1を水平位置から自由運動させる 
%
%       Notice!
%       call acrobot3_eq0.m
%
%            2005.12.10                          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 ----
th0=-1*pi/2; %angle of Link1(rad)   初期値の角度を入力　-3.14/2:真横  (真上基準）
d_th0= 0; %velocity of Link1(rad)         

x0=[th0  d_th0 ]; %initial condition


%---mechanical parameter---
m = 0.16;       %Mass of Link1[Kg]
l = 0.083;      %length of Link1[m]
a=  0.067;      %length to the center of gravity of Link1[m]
I=  7.05e-5;    %Link1 inertia moment of the circumference of the center of gravity[kgm-2]3.18e-4
c=  2.0e-4;     %[Nms/rad]Link1 1.49e-4
g = 9.8;        %[m/s^2] acceleration of gravity


%-----------------  Non linear system------------------------------------------------
%---definition---
%振子角度　振子角速度  振子角加速度   
%---y(1,1)   y(2,1)     y(3,1)      
%           dy(1,1)     dy(2,1)     
%-------------------------------------------------------------------------------------

%---Y(1,1)     Y(2,1)         Y(3,1)           
%振子角度　　   振子角速度     振子角加速度
%--------------------------------------------------------------------------

disp('It is under calculation now!!')
ni=1;
tend=6000;  %6000   応答時間に合わせて変える
flag=0;
while(ni <= tend) 
    
             [Tn,Yn]=ode45(@acrobot3_eq0,[0 sampling],x0,[],c,m,a,l,I,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(TT,YY(:,1));
axis([0  sampling*ni -8  +8])
xlabel('Time[s]');
ylabel('Angle of Link1[rad]')
legend('Link1 q1','Link2 q2',2);

figure(2)
plot(TT,180/pi * YY(:,1));
axis([0  sampling*ni -90  +90])
xlabel('Time[s]');
ylabel('Angle of Link1[degree]')
title('Angle of Link1')








