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


%definição dos valores das constantes
dx=-30;
dy=30;
r2=4;
r3=25;
r4=30;
r5=20;
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)+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)
altura=max(yt)
var_CM=max(y0)-min(y0)
%save('c:\quinama_conf.txt','passo','altura','-ascii','-append');
for h=0:2
   
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
hold on
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.')

plot([x0(j)+passo-10,xq(j)+passo-10,xr(j)+passo-10,xp(j)+passo-10],[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],[y0(j),yq(j),yr(j),yp(j)],'m',...
   [xq(j)+passo-10,xt(1:j)+passo-10],[yq(j),yt(1:j)],'r.',[xq(j)+passo-10,xt(j)+passo-10],[yq(j),yt(j)],'m',...
   x0(1:j)+passo-10,y0(1:j),'g.')

if j>=round(length(teta2)/2);
   v=j-round(length(teta2)/2)+1;
plot([x0(v)+passo-5,xq(v)+passo-5,xr(v)+passo-5,xp(v)+passo-5],[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],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)+passo-5,xt(1:v)+passo-5],[yq(v),yt(1:v)],'r.',[xq(v)+passo-5,xt(v)+passo-5],[yq(v),yt(v)],'m',...
   x0(1:v)+passo-5,y0(1:v),'g.')
   
else
   v=j+round(length(teta2)/2);
plot([x0(v)-5,xq(v)-5,xr(v)-5,xp(v)-5],[y0(v),yq(v),yr(v),yp(v)],'ro',...
   [x0(v)-5,xq(v)-5,xr(v)-5,xp(v)-5],[y0(v),yq(v),yr(v),yp(v)],'m',...
   [xq(v)-5,xt(1:v)-5],[yq(v),yt(1:v)],'r.',[xq(v)-5,xt(v)-5],[yq(v),yt(v)],'m',...
   x0(1:v)-5,y0(1:v),'g.')
   
end
 
axis equal
axis([-200 0 0 50]);
grid on
pause(0.0001)
clf
end
end

