%««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
%«««««««			   	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=10;
dy=12;
r2=4;
r3=15;
r4=15;
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;

larg=ones(size(teta2))*60;

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)+pi+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)+pi+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)+pi+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)+pi+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)+pi+ang_perna);
end   
end


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



for j=1 :15: length(teta2)
hold on
%lado esquerdo

plot3([x0(j),xq(j),xr(j),xp(j)],[0,0,0,0],[y0(j),yq(j),yr(j),yp(j)],'ro',...
   [x0(j),xq(j),xr(j),xp(j)],[0,0,0,0],[y0(j),yq(j),yr(j),yp(j)],'m',...
   [xq(j),xt(1:j)],[0,zeros(1,j)],[yq(j),yt(1:j)],'r.',[xq(j),xt(j)],[0,0],[yq(j),yt(j)],'m',...
   x0(1:j),zeros(1,j),y0(1:j),'g.')

plot3([x0(j)+passo-10,xq(j)+passo-10,xr(j)+passo-10,xp(j)+passo-10],[0,0,0,0],[y0(j),yq(j),yr(j),yp(j)],'ro',...
   [x0(j)+passo-10,xq(j)+passo-10,xr(j)+passo-10,xp(j)+passo-10],[0,0,0,0],[y0(j),yq(j),yr(j),yp(j)],'m',...
   [xq(j)+passo-10,xt(1:j)+passo-10],[0,zeros(1,j)],[yq(j),yt(1:j)],'r.',[xq(j)+passo-10,xt(j)+passo-10],[0,0],[yq(j),yt(j)],'m',...
   x0(1:j)+passo-10,zeros(1,j),y0(1:j),'g.')


if j>=round(length(teta2)/2);
   v=j-round(length(teta2)/2)+1;
plot3([x0(v)+passo-5,xq(v)+passo-5,xr(v)+passo-5,xp(v)+passo-5],[0,0,0,0],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)+passo-5,xq(v)+passo-5,xr(v)+passo-5,xp(v)+passo-5],[0,0,0,0],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)+passo-5,xt(1:v)+passo-5],[0,zeros(1,v)],[yq(v),yt(1:v)],'r.',[xq(v)+passo-5,xt(v)+passo-5],[0,0],[yq(v),yt(v)],'m',...
   x0(1:v)+passo-5,zeros(1,v),y0(1:v),'g.')

else
   v=j+round(length(teta2)/2);
plot3([x0(v)-5,xq(v)-5,xr(v)-5,xp(v)-5],[0,0,0,0],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)-5,xq(v)-5,xr(v)-5,xp(v)-5],[0,0,0,0],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)-5,xt(1:v)-5],[0,zeros(1,v)],[yq(v),yt(1:v)],'r.',[xq(v)-5,xt(v)-5],[0,0],[yq(v),yt(v)],'m',...
   x0(1:v)-5,zeros(1,v),y0(1:v),'g.')
   
end
 
%lado direito 
plot3([x0(j)-5,xq(j)-5,xr(j)-5,xp(j)-5],[larg(j),larg(j),larg(j),larg(j)],[y0(j),yq(j),yr(j),yp(j)],'ro',...
   [x0(j)-5,xq(j)-5,xr(j)-5,xp(j)-5],[larg(j),larg(j),larg(j),larg(j)],[y0(j),yq(j),yr(j),yp(j)],'m',...
   [xq(j)-5,xt(1:j)-5],[larg(j),larg(1:j)],[yq(j),yt(1:j)],'r.',[xq(j)-5,xt(j)-5],[larg(j),larg(j)],[yq(j),yt(j)],'m',...
   x0(1:j)-5,larg(1:j),y0(1:j),'g.')

if j>=round(length(teta2)/2);
   v=j-round(length(teta2)/2)+1;
plot3([x0(v),xq(v),xr(v),xp(v)],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v),xq(v),xr(v),xp(v)],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v),xt(1:v)],[larg(v),larg(1:v)],[yq(v),yt(1:v)],'r.',[xq(v),xt(v)],[larg(v),larg(v)],[yq(v),yt(v)],'m',...
   x0(1:v),larg(1:v),y0(1:v),'g.')
plot3([x0(v)+passo-10,xq(v)+passo-10,xr(v)+passo-10,xp(v)+passo-10],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)+passo-10,xq(v)+passo-10,xr(v)+passo-10,xp(v)+passo-10],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)+passo-10,xt(1:v)+passo-10],[larg(v),larg(1:v)],[yq(v),yt(1:v)],'r.',[xq(v)+passo-10,xt(v)+passo-10],[larg(v),larg(v)],[yq(v),yt(v)],'m',...
   x0(1:v)+passo-10,larg(1:v),y0(1:v),'g.')
   
else
   v=j+round(length(teta2)/2);
plot3([x0(v)-passo,xq(v)-passo,xr(v)-passo,xp(v)-passo],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)-passo,xq(v)-passo,xr(v)-passo,xp(v)-passo],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)-passo,xt(1:v)-passo],[larg(v),larg(1:v)],[yq(v),yt(1:v)],'r.',[xq(v)-passo,xt(v)-passo],[larg(v),larg(v)],[yq(v),yt(v)],'m',...
   x0(1:v)-passo,larg(1:v),y0(1:v),'g.')
plot3([x0(v)-10,xq(v)-10,xr(v)-10,xp(v)-10],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)-10,xq(v)-10,xr(v)-10,xp(v)-10],[larg(v),larg(v),larg(v),larg(v)],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)-10,xt(1:v)-10],[larg(v),larg(1:v)],[yq(v),yt(1:v)],'r.',[xq(v)-10,xt(v)-10],[larg(v),larg(v)],[yq(v),yt(v)],'m',...
   x0(1:v)-10,larg(1:v),y0(1:v),'g.')

end 
 
axis equal
axis([-200 100 0 100 0 50]);
view(30,140)
grid on
pause(0.01)
clf
end
end

