%««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
%«««««««			   	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);
%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)-pi/8)+xt(100);
yq(n) = r2*sin(teta2(n))-r2*sin(teta2(n))+r5*sin(teta3(n)-pi/8);

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

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

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

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

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

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

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

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


% animação da rotação da perna
passo=xt(100)-xt(1)
altura_ao_solo=max(yt)
oscilacao_do_CM=max(y0)-min(y0)
for h=0:1
   
xq=xq+passo; 
xr=xr+passo;
xp=xp+passo;
x0=x0+passo;
xt=xt+passo;   



for j=1 :5: length(teta2)
   %j=length(teta2)-l+1
clf
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',...
   [xq(j),xt(1:j)],[yq(j),yt(1:j)],'r.',[xq(j),xt(j)],[yq(j),yt(j)],'m',...
   x0(1:j),y0(1:j),'g.')

axis equal
axis([-150 0 -5 50]);
grid on
pause(0.01)

end
end

