%««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
%«««««««			   	Cinemática do Membro de um Hexapode              »»»»»»»
%«««««««      15/11/2001-projecto fim de curso- Ramo Automação      »»»»»»»
%«««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
clear all
clc

l1=20;
l2=4;
l3=20;


teta=2*pi:-pi/200:0;

xo=0 + (teta<pi).*(2.*l2.*cos(teta)+2*l2);
yo=0 + (teta<pi).*(2.*l2.*sin(teta));

xp= 0 + (teta<pi).*(2.*l2.*cos(teta)+2*l2);
yp=(teta>=pi).*l1+(teta<pi).*(l1+2.*l2.*sin(teta));

xq= 0 + (teta<pi).*(2.*l2.*cos(teta)+2*l2);
yq=(teta>=pi).*(l1+l3)+(teta<pi).*(l1+l3+2.*l2.*sin(teta));

xr=(teta>=pi).*(l2.*cos(teta+pi))+(teta<pi).*(l2.*cos(teta)+2*l2);
yr=(teta>=pi).*(l1+l2.*sin(teta+pi))+(teta<pi).*(l1+l2.*sin(teta));

xs=(teta>=pi).*(l2.*cos(teta+pi))+(teta<pi).*(l2.*cos(teta)+2*l2);
ys=(teta>=pi).*(l1+l3+l2.*sin(teta+pi))+(teta<pi).*(l1+l3+l2.*sin(teta));


passo=2*l2;

for m=1:4
	for j=1:5:length(teta)

   plot([xo(j),xp(j),xq(j),xs(j)],[yo(j),yp(j),yq(j),ys(j)],'r.',[xp(j),xr(j)],[yp(j),yr(j)],'r.',...
      [xo(j),xp(j),xq(j),xs(j)],[yo(j),yp(j),yq(j),ys(j)],'r',[xp(j),xr(j)],[yp(j),yr(j)],'r',...
      xo(1:j),yo(1:j),'m:',xr(1:j),yr(1:j),'k:')
   

	axis equal
	axis([-10 60 0 50]);
   grid on
   axis off
	pause(0.001)
   clf
	end
  
   xo=xo+2*passo;
   xp=xp+2*passo;
   xq=xq+2*passo;
   xr=xr+2*passo;
   xs=xs+2*passo;

end
