function ZieglerND
%ZieglerND.m solves the nondimensional nonlinear ODE govorning the Zeigler 
%pendulum.  The given system is unstable.  Removing damping (i.e., setting
%c1 and c2 to 0) stabalizes the system.

%Parameters:
a=1;      %follower force angle multiplier.
L=1;      %ratio of link lengths.
m=1;      %translated ratio of masses: m1/m2 - 1
k=1;      %ratio of joint spring constants: k1/k2
c1=0.05;  %damping parameter for the first joint.   
c2=0.05;  %damping paramter for the second joint.
F=2;      %follower force magnitude.
g=0;      %gravity.

%Initial Conditions:
ph1=0.001;
ph2=-0.001;
ph1d=0;
ph2d=0;

opts=odeset('reltol',1e-9);
[t,x]=ode45(@zieg,linspace(0,150,500),[ph1 ph2 ph1d ph2d],opts,...
            a,L,m,k,c1,c2,F,g);

%Animate!
fig1=figure;  
set(fig1,'color',[1 1 1],'backingstore','off','Doublebuffer','on');
links=line('xdata',[],'ydata',[],'color',[0 0 1]);
joints=line('xdata',[],'ydata',[],'linestyle','none','marker','o',...
            'color',[1 0 0]);
originx=line('xdata',0.5*[-1 1],'ydata',[0 0],'color',[0 0 0]);
originy=line('xdata',[0 0],'ydata',0.5*[-1 1],'color',[0 0 0]);
set(gca,'visible','off','view',[90 90],...
        'xlim',[-0.5 1.1*(1+L)],'ylim',(1+L)*[-1 1],...
        'DataAspectRatio',[1 1 1]);
for i=1:length(t)
    p1=[cos(x(i,1));sin(x(i,1))];
    p2=p1+L*[cos(x(i,2));sin(x(i,2))];  
    set(links,'xdata',[0 p1(1) p2(1)],'ydata',[0 p1(2) p2(2)])
    set(joints,'xdata',[p1(1) p2(1)],'ydata',[p1(2) p2(2)])
    pause(0.05)
    drawnow
end

%Energy
KE=0.5*L^2*(m+2)*x(:,3).^2+0.5*x(:,4).^2+...
   L*x(:,3).*x(:,4).*cos(x(:,1)-x(:,2));    %Kinetic energy
PEs=0.5*(k*x(:,1).^2+(x(:,2)-x(:,1)).^2);   %PE: joint springs
PEg=-g*(m+2)*L*cos(x(:,1))-g*cos(x(:,2));   %PE: gravity
PEg=PEg-PEg(1);

%Plot of energy variation:
fig2=figure;
set(fig2,'color',[1 1 1]);
plot(t,KE,'r:',t,PEs+PEg,'b--',t,KE+PEs+PEg,'k')
title('Energy versus Time')
xlabel('Time')
ylabel('Energy')
legend('Kinetic Energy','Potential Energy','Total Energy')

%Logarithmic plot of total energy:
fig3=figure;
set(fig3,'color',[1 1 1])
plot(t,log(KE+PEs+PEg)-log(KE(1)+PEs(1)+PEg(1)),'k');
title('Log(Total Energy) versus Time')
xlabel('Time')
ylabel('Log(Total Energy)')
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xp=zieg(t,x,a,L,m,k,c1,c2,F,g)

ph1=x(1);
ph2=x(2);
ph1d=x(3);
ph2d=x(4);

M(2,2)=1;
M(1,1)=L^2*(m+2);
M(1,2)=L*cos(ph1-ph2);
M(2,1)=M(1,2);

phdd=M\(-L*sin(ph1-ph2)*[ph2d^2;-ph1d^2]...
        +[-(k+1) 1;1 -1]*[ph1;ph2]...
        +[-(c1+c2) c2;c2 -c2]*[ph1d;ph2d]...
        +F*[sin(ph1-a*ph2);sin((1-a)*ph2)/L]...
        -g*[(m+2)*L*sin(ph1);sin(ph2)]);

xp=[ph1d;ph2d;phdd];