close all

clc
clear all

t_1_i = 0; %angulos a serem incrementados at� o valor imputado

t_2_i = 0;
t_3_i= 0;

link_1 = 5; %comprimentos dos links

link_2 = 2;
link_3 = 1;

t_1_o = 0; %angulos inicial at� o imputado pelo usuario

t_2_o = 0;
t_3_o = 0;

%posi��o de cada ponto em rela��o ao eixo anterior------------------------

px_1 = link_1*cosd(t_1_i);
py_1 = link_1*sind(t_1_i);

px_2 = (link_2)*cosd(t_2_i);
py_2 = (link_2)*sind(t_2_i);

px_3 = (link_3)*cosd(t_3_i);
py_3 = (link_3)*sind(t_3_i);
%-------------------------------------------------------------------------
%vetores posi��o----------------------------------------------------------
p_1 = [px_1; py_1; 0; 1]; %[x;y;z=0;1]
p_2 = [px_2; py_2; 0; 1];
p_3 = [px_3; py_3; 0; 1];
%-------------------------------------------------------------------------
I = 200;

t_1 = input('Insira o primeiro �ngulo: ');

t_2 = input('Insira o segundo �ngulo: ');
t_3 = input('Insira o terceiro �ngulo: ');

h = figure(1); %abre a janela grafica 1

for k = 0:I

t_1_i = ((I - (I - k))/I)*t_1 +t_1_o; %as porcentagens do angulo

computado de 0% at� 100% do valor
T1 = [cosd(t_1_i), -sind(t_1_i),0, 0; sind(t_1_i), cosd(t_1_i),0, 0; 0,0, 1,
0;0,0,0,1];
p_1_i = T1*p_1; %novas posi��es de acordo com os
incrementos de angulo

t_2_i = ((I - (I - k))/I)*t_2 +t_2_o;

T2 = [cosd(t_2_i), -sind(t_2_i),0, link_1; sind(t_2_i), cosd(t_2_i),0,0; 0,0,
1,0;0,0,0,1];
p_2_i = T1*(T2*(p_2));

t_3_i = ((I - (I - k))/I)*t_3 +t_3_o;

T3 = [cosd(t_3_i), -sind(t_3_i),0,link_2; sind(t_3_i), cosd(t_3_i),0,0; 0,0,
1,0;0,0,0,1];
p_3_i = T1*(T2*(T3*p_3));

%representa��o de cada link no plano do gr�fico-----------------------

 link1_x = [0,p_1_i(1,1)];
link1_y = [0,p_1_i(2,1)];

link2_x = [p_1_i(1,1),p_2_i(1,1)];
link2_y = [p_1_i(2,1),p_2_i(2,1)];

link3_x = [p_2_i(1,1),p_3_i(1,1)];
link3_y = [p_2_i(2,1),p_3_i(2,1)];
%---------------------------------------------------------------------
plot(link1_x,link1_y,'-o',link2_x,link2_y,'-o',link3_x,link3_y,'-
o','linewidth',2);
hold on
plot([0,0],[0,3],'-^k',[0,3],[0,0],'->k','linewidth',0.01)
axis ([-(link_1+link_2+link_3) (link_1+link_2+link_3) -(link_1+link_2+link_3)
(link_1+link_2+link_3)])
grid on

title('Exerc�cio 01');
xlabel('Eixo X');
ylabel('Eixo Y');

end_effectorx=num2str(p_3_i(1,1));
e_e_x=text(-7.5,-5,['Posi��oTool_x=
',end_effectorx],'BackgroundColor','w','EdgeColor','b','FontSize',14);
end_effectory=num2str(p_3_i(2,1));
E_e_y=text(-7.5,-7,['Posi��oTool_y=
',end_effectory],'BackgroundColor','w','EdgeColor','b','FontSize',14);
drawnow

%redesenha o gr�fico a cada incremento de angulo sem fechar a janela--

if k<I
clf(h);
end
%---------------------------------------------------------------------
end

fprintf('\nMatrizes de transforma��o:\n')

T1
T2
T3

T = T1*(T2*T3)