%% Run your simulink model before using this code.
%% You need to get the position vector y and the angle vector
%% theta in the workspace

ptime=0.01;%If you want to make the animation slower, choose larger 
           % value of ptime 

frame=[[-6 6 6 -6 -6];[1 1 -1 -1 1]*0.35];
%xx=xx*0.001;frame=frame*0.001;

box=[[-1 1 1 -1 -1]*0.1;[1 1 0 0 1]*0.08+0.02];
circ1=[sin(0:0.1:pi*2)*0.01-0.05;cos(0:0.1:pi*2)*0.01+0.01];
circ2=[sin(0:0.1:pi*2)*0.01+0.05;cos(0:0.1:pi*2)*0.01+0.01];
plot(frame(1,:),frame(2,:))
hold 

pause

body=line('color','b','lineStyle','-','erase','xor','xdata',[],'ydata',[]);
body1=line('color','c','lineStyle','-','erase','xor','xdata',[],'ydata',[]);
head=line('color','r','lineStyle','-','erase','xor','xdata',[],'ydata',[]);
body2=line('color','c','lineStyle','-','erase','xor','xdata',[],'ydata',[]);
body3=line('color','g','lineStyle','-','erase','xor','xdata',[],'ydata',[]);

N=length(y);

for k=1:N;
     boxa=[box(1,:)+y(k);box(2,:)];
     circ1a=[circ1(1,:)+y(k);circ1(2,:)];
     circ2a=[circ2(1,:)+y(k);circ2(2,:)];
     p1=[0+y(k);0.1];p2=p1+0.2*[sin(theta(k));cos(theta(k))];
     circ0=[sin(0:0.1:pi*2)*0.02+p2(1);cos(0:0.1:pi*2)*0.02+p2(2)];

%set(head,'xdata',x(mod(j,N)+1),'ydata',y(mod(j,N)+1)+aa(j))
set(body,'xdata',boxa(1,:),'ydata',box(2,:))
set(body1,'xdata',circ1a(1,:),'ydata',circ1(2,:))
set(body2,'xdata',circ2a(1,:),'ydata',circ2(2,:))
set(body3,'xdata',[p1(1) p2(1)],'ydata',[p1(2) p2(2)])
set(head,'xdata',circ0(1,:),'ydata',circ0(2,:))

if k==1; pause
else pause(ptime)
end
drawnow
end
hold off