%««««««««««««««««««««««««««««««««««««»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»»
%«««««««			   	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

r1=2;
r2=1.5;
l1=2;
dx=1;
dy=4;

%cálculo de beta em função de teta

teta=0:pi./100:2.*pi;
k1 = 2.*r1.*(r2.*cos(teta)-dx);
k2 = 2.*r1.*(r2.*sin(teta)-dy);
k3 = r1.^2+r2.^2-dx.^2-dy.^2-l1.^2;

% soluyção da equação: k1.*cos(beta)+k2.*sin(beta)=k3
teta=0:pi./100:2.*pi;
beta=atan2((k2-(k1.^2+k2.^-k3.^2).^1/2),(k1+k3));

%cálculo dos pontos de rotação da perna

x3 = r2*cos(teta);
y3 = r2*sin(teta);

x2 = dx+r1*cos(beta);
y2 = dy+r1*sin(beta);

x1 = dx*ones(1,length(beta));
y1 = dy*ones(1,length(beta));

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

% animação da rotação da perna

for j=1 :5: length(beta)
   
plot([x0(j),x3(j),x2(j),x1(j)],[y0(j),y3(j),y2(j),y1(j)])
axis equal
axis([-10 10 -10 10]);
grid on
pause(0.01)
end