function twolink_ani(tout,xout)
% function twolink_ani(tout,xout)
%
% This function plots acrobot motions, given outputs from ode45.
% Load example data stored in twolink_data.mat to test this function.
% Katie Byl, UCSB, June 6, 2012. (ECE 179d)

t1 = tout(1);
dt = 1/20;
tani = t1:dt:max(tout);
figure(22); clf; hold on
L1 = 1; L2 = 1; LW = 3; % line width
for n=1:length(tani);
    th1 = interp1(tout,xout(:,1),tani(n));
    th2 = interp1(tout,xout(:,2),tani(n));
    x1 = L1*cos(th1); y1 = L1*sin(th1);
    x2 = x1+L2*cos(th1+th2); y2 = y1+L2*sin(th1+th2);
    if n==1
        p1 = plot([0 x1],[0 y1],'b-','LineWidth',LW);
        p2 = plot([x1 x2],[y1 y2],'r-','LineWidth',LW);
    else
        set(p1,'XData',[0 x1],'YData',[0 y1]);
        set(p2,'XData',[x1 x2],'YData',[y1 y2]);
    end
    axis image;
    axis([-2 2 -2 2]);
    title(['Time = ',num2str(tani(n),'%5.1f'), ' sec'])
    if n>1, pause(max(0,(n-1)*dt-toc)), drawnow 
    else drawnow, tic 
    end
    drawnow
end
