%----------------------------------------------------------------  
%       線形倒立振子(Linear Inverted Pendulum)のふるまい
%         
%      
%       
%       Notice!
%       call  LIV1_eq1.m
%
%            2005.5.18                          produced by kizawa
%--------------------------------------------------------------------------

clc
clear all
format long e

%==========================================================
%
%                       g
%                dd_x= --- x
%                       z
%===========================================================
%---phisical parameter---%
g=9.8;
z=0.8;
%----setup---%

sampling=0.001;%サンプリング時間[sec]
kaisu=1000;
timespan=sampling*kaisu;% シミュレーション時間
%--- initial state ----
x0=[-0.2    %変位の初期値[m]
     0.8   ];  %速度の初期値[m/s]

%---微分方程式を解くメインプログラム----%
ni=1;
while(ni<1000)
   
[Tn,Yn]=ode45(@LIV1_eq1,[0 sampling],x0,[],g,z);%微分を数値的に解く関数,分割数はMTALABまかせ
                                                  %微分方程式はサブルーチン化LIV1_eq1.m 
           if ni==1
              YY(1,:)=Yn(1,1:2);     %初期値のx方向の変位と速度
              TT(1,:)=Tn(1,1);       %初期値の時間
           else    
             YY(ni,:)=Yn(size(Yn,1),1:2);   %計算後の角度と角速度(分割数の最大値を割り出す)
             TT(ni,1)=sampling*ni;          %計算後の時間
           end
           %---- Up data------
           x0=Yn(size(Yn,1),1:2);    %初期値の入れ替え
           ni=ni+1;                  %次のステップへ
end  
%-----グラフを描く-----%
figure(1)
zz=z*ones(size(TT,1),1);
plot(TT,YY(:,1),'-b')
xlabel('Time[sec]');
ylabel('x[m]')
axis([0  sampling*ni -0.2  +0.2])
%----Animation---%
figure(2)
xx=YY(:,1);
zz=z*ones(size(TT,1),1);

h=plot([0;0],[xx(1),zz(1)],'linewidth',2,'Marker','o');
axis([-0.4  0.5  0  1]);
xlabel('X[m]');
ylabel('Z[m]');
title('線形倒立振子の振る舞い x(0)=-0.2, d_x(0)=0.8 z=0.8')
for i=1 : size(TT)
    set(h,'xdata',[0,xx(i)],'ydata',[0,zz(i)],'Erasemode','xor');
    drawnow
    pause(0.1);
    grid on
end    