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

%definição dos valores das constantes
dx=15;
dy=12;
r2=4;
r3=15;
r4=20;
r5=15;
r1=abs(sqrt(dx^2+dy^2));
teta1=atan2(dy, dx);
ang_perna=-pi/8;

%cálculo de teta4 e de teta3 em função de teta2
teta2=0:pi/100:2*pi;
A = 2*r1*r4*cos(teta1)-2*r2*r4*cos(teta2);
B = 2*r1*r4*sin(teta1)-2*r2*r4*sin(teta2);
C = r1^2+r2^2+r4^2-r3^2-2*r1*r2*(cos(teta1)*cos(teta2)+sin(teta1)*sin(teta2));

teta4 = 2*atan2(-B-sqrt(B.^2-C.^2+A.^2),C-A);

teta3 = atan2(r1*sin(teta1)+r4*sin(teta4)-r2*sin(teta2),...
   r1*cos(teta1)+r4*cos(teta4)-r2*cos(teta2));

%Cálculo dos pontos das juntas
for n=1:length(teta2)
   
   if teta2(n)>=pi
   
xq(n) = r2*cos(teta2(n))-r2*cos(teta2(n))+r5*cos(teta3(n)+ang_perna)+xt(100);
yq(n) = r2*sin(teta2(n))-r2*sin(teta2(n))+r5*sin(teta3(n)+ang_perna);

xr(n) = r1*cos(teta1)+r4*cos(teta4(n))-r2*cos(teta2(n))+r5*cos(teta3(n)+ang_perna)+xt(100);
yr(n) = r1*sin(teta1)+r4*sin(teta4(n))-r2*sin(teta2(n))+r5*sin(teta3(n)+ang_perna);

xp(n) = r1*cos(teta1)-r2*cos(teta2(n))+r5*cos(teta3(n)+ang_perna)+xt(100);
yp(n) = r1*sin(teta1)-r2*sin(teta2(n))+r5*sin(teta3(n)+ang_perna);

x0(n) = -r2*cos(teta2(n))+r5*cos(teta3(n)+ang_perna)+xt(100);
y0(n) = -r2*sin(teta2(n))+r5*sin(teta3(n)+ang_perna);

xt(n) =r2*cos(teta2(n))-r5*cos(teta3(n)+ang_perna)-r2*cos(teta2(n))+r5*cos(teta3(n)+ang_perna)+xt(100); 
yt(n) =r2*sin(teta2(n))-r5*sin(teta3(n)+ang_perna)-r2*sin(teta2(n))+r5*sin(teta3(n)+ang_perna);
else
xq(n) = r2*cos(teta2(n))-r2*cos(teta2(n)+pi)+r5*cos(teta3(n)+pi+ang_perna);
yq(n) = r2*sin(teta2(n))+((-r2*sin(teta2(n)+pi)+r5*sin(teta3(n)+ang_perna)));

xr(n) = r1*cos(teta1)+r4*cos(teta4(n))-r2*cos(teta2(n)+pi)+r5*cos(teta3(n)+pi+ang_perna);
yr(n) = r1*sin(teta1)+r4*sin(teta4(n))+(-r2*sin(teta2(n)+pi)+r5*sin(teta3(n)+ang_perna));

xp(n) = r1*cos(teta1)-r2*cos(teta2(n)+pi)+r5*cos(teta3(n)+pi+ang_perna);
yp(n) = r1*sin(teta1)+(-r2*sin(teta2(n)+pi)+r5*sin(teta3(n)+ang_perna));

x0(n) = (-r2*cos(teta2(n)+pi)+r5*cos(teta3(n)+pi+ang_perna));
y0(n) = (-r2*sin(teta2(n)+pi)+r5*sin(teta3(n)+ang_perna));

xt(n) =r2*cos(teta2(n))-r5*cos(teta3(n)+ang_perna)-r2*cos(teta2(n)+pi)+r5*cos(teta3(n)+pi+ang_perna); 
yt(n) =r2*sin(teta2(n))-r5*sin(teta3(n)+ang_perna)+(-r2*sin(teta2(n)+pi)+r5*sin(teta3(n)+ang_perna));
end   
end


%cálculo dos parametros de avaliação
passo=xt(100)-xt(1)
altura=max(yt)
var_CM=max(y0)-min(y0)
comprimento=abs(2*passo-2)

% animação da rotação da perna
for h=0:2
   
	xq=xq+passo; 
	xr=xr+passo;
	xp=xp+passo;
	x0=x0+passo;
	xt=xt+passo;   



	for j=1 :1: length(teta2)
      
      clf
	   hold on
   	patch([xp(j)+2,xp(j)+2*passo,xp(j)+2*passo,xp(j)+2],...
	   [yp(j)+1,yp(j)+1,yp(j)-15,yp(j)-15],'y')

   
		plot([x0(j),xq(j),xr(j),xp(j)],[y0(j),yq(j),yr(j),yp(j)],'ro',...
	   [x0(j),xq(j),xr(j),xp(j)],[y0(j),yq(j),yr(j),yp(j)],'m',...
   	xt(1:j),yt(1:j),'r-',[xq(j),xt(j)],[yq(j),yt(j)],'m',...
	   x0(1:j),y0(1:j),'k-')

		plot([x0(j)+passo-12,xq(j)+passo-12,xr(j)+passo-12,xp(j)+passo-12],[y0(j),yq(j),yr(j),yp(j)],'ro',...
	   [x0(j)+passo-12,xq(j)+passo-12,xr(j)+passo-12,xp(j)+passo-12],[y0(j),yq(j),yr(j),yp(j)],'m',...
   	xt(1:j)+passo-12,yt(1:j),'r-',[xq(j)+passo-12,xt(j)+passo-12],[yq(j),yt(j)],'m',...
	   x0(1:j)+passo-12,y0(1:j),'k-')

			if j>=round(length(teta2)/2);
            
            v=j-round(length(teta2)/2)+1;
				plot([x0(v)+passo-6,xq(v)+passo-6,xr(v)+passo-6,xp(v)+passo-6],[y0(v),yq(v),yr(v),yp(v)],'ro',...
			   [x0(v)+passo-6,xq(v)+passo-6,xr(v)+passo-6,xp(v)+passo-6],[y0(v),yq(v),yr(v),yp(v)],'m',...
			   xt(1:v)+passo-6,yt(1:v),'r-',[xq(v)+passo-6,xt(v)+passo-6],[yq(v),yt(v)],'m',...
			   x0(1:v)+passo-6,y0(1:v),'k-')
   
			else
			   v=j+round(length(teta2)/2);
				plot([x0(v)-6,xq(v)-6,xr(v)-6,xp(v)-6],[y0(v),yq(v),yr(v),yp(v)],'ro',...
			   [x0(v)-6,xq(v)-6,xr(v)-6,xp(v)-6],[y0(v),yq(v),yr(v),yp(v)],'m',...
			   xt(1:v)-6,yt(1:v),'r-',[xq(v)-6,xt(v)-6],[yq(v),yt(v)],'m',...
			   x0(1:v)-6,y0(1:v),'k-')
   
			end
		axis equal
		axis([-180 0 0 50]);
      grid on
      axis off
      if h==1;MM(j)=getframe;end;
		pause(0.01)

	end
end

