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

clc 
clf 
clear all

%definição das constantes simbolicas
syms r1 r2 l1 l2 dy dx

%definição das variáveis simbolicas


%teta=sym('teta','real');

%definição dos valores das constantes

r1=3;
r2=1.5;
l1=3;
l2=3;
dx=1;
dy=4;

% esta condição resulta da raiz em causa ser positiva para teta entre 0 e 180 
% e negativa entre 180 e 360 assim influencia o valor de x2

j=0;
for u=0:pi/10:2*pi
x2=sym('x2','real');
y2=sym('y2','real');
j=j+1;   
teta=u;
	if teta<=pi;
		x2=(r1^2-l1+dx^2-dy^2+r2^2+r2^2*cos(teta)^2-cos(teta)+2*y2*(dy+sqrt(r2^2-cos(teta)^2)))/2;
		else
      x2=(r1^2-l1+dx^2-dy^2+r2^2+r2^2*cos(teta)^2-cos(teta)+2*y2*(dy-sqrt(r2^2-cos(teta)^2)))/2;
   end
   %esta condição é para ter em conta a transição de horizontalidade de Y2
   
   if (r2*sin(teta)+l1)>=dy;   
     y2=solve('0=-y2+dy+(-x2^2+2*x2*dx-dx^2+r1^2)^(1/2)','y2');
   else
     y2=solve('0=-y2+dy-(-x2^2+2*x2*dx-dx^2+r1^2)^(1/2)','y2');
   end
subs(y2);
y2(j)=numeric(y2);
x2(j)=sqrt(y2(j)^2+r1^2);
x3(j)=r2*cos(teta);
y3(j)=r2*sin(teta);
end

x1=dx;
y1=dy;

%visualização/animação dos segmentos de recta 1_2 2_3 3_0
for i=1:20
   grid on
	axis('equal')
	axis([-4 4 -5 10])
   hold on
   plot([0,x3(i)],[0,y3(i)],'m.',[0,x3(i)],[0,y3(i)],'b')
   plot([x3(i),x2(i)],[y3(i),y2(i)],'m.',[x3(i),x2(i)],[y3(i),y2(i)],'b')
   plot([x2(i),x1],[y2(i),y1],'m.',[x2(i),x1],[y2(i),y1],'b')
   clf
end
