%««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
%«««««««			   	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=-4;
dy=10;
r1=abs(sqrt(dx^2+dy^2));
r2=4;
r3=10;
r4=8;
r5=20;
teta1=atan2(dy,dx);
%cálculo de teta4 e de teta3 em função de teta2

teta2=0:pi/100: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

xq = r2*cos(teta2);
yq = r2*sin(teta2);

xr = r1*cos(teta1)+r4*cos(teta4);
yr = r1*sin(teta1)+r4*sin(teta4);

xp = r1*cos(teta1)*ones(1,length(teta2));
yp = r1*sin(teta1)*ones(1,length(teta2));

x0 = zeros(1,length(teta2));
y0 = zeros(1,length(teta2));

xt = r2*cos(teta2)-r5*cos(teta3);
yt = r2*sin(teta2)-r5*sin(teta3);
% animação da rotação da perna

for j=1 :5: length(teta2)
  %j=length(teta2)-l+1
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')
axis equal
axis([-50 50 -50 50]);
grid on
pause(0.01)
end


