% Demo_03

% Notese que en el Arduino:

% - "println": Prints data to the serial port as human-readable ASCII
% text followed by a carriage return charact1er (ASCII 13, or '\r')
% and a newline character (ASCII 10, or '\n')
% - Para que todo sea simple es mejor trabajar con envios de 1 byte tipo
% "uchar"
% - Este demo trabaja con "Demo_03" del arduino
clc; clear all;

%----------------------------------------------------------------------%
% 1. CONFIGURACION DEL PUERTO SERIAL
%----------------------------------------------------------------------%
delete(instrfind({'Port'},{'COM7'}));
s1 = serial('COM7');
s1.BaudRate=9600;
fopen(s1);

%----------------------------------------------------------------------%
% 2. CODIGO PRINCIPAL
%----------------------------------------------------------------------%
%while(1)
% APLICAR CINEMATICA INVERSA

%
%angle_1 = input('Angulo de la junta 1: ');
%angle_2 = input('Angulo de la junta 2: ');
%if(angle_1==-2 || angle_2==-2)
% disp('Finalizando')
% break
%end
% ROBOT PUMA 560 -----------}--
%-----------------------
%Autor: MAX JHON PEPE
%Tema: Cinemtica de un manipulador antropomorfo de 3GDL
%clc; clear all; close all;
while(1)
%--------------------------------------------------------%
% 1. CONFIGURACIN INICIAL
%--------------------------------------------------------%
% 1.1. POSICIN DESEADA
xc = input('xc: ');
yc = input('yc: ');
zc = input('zc: ');
%xc=20;
%yc=-10;
%zc=10;

% 1.2. PLOTEO DEL ENTORNO

T0=eye(4);
plot_frame(T0,'color','k','view','auto');
axis([-30 30 -30 30 0 60]);
hold on;
grid on;
xlabel('x(m)');
ylabel('y(m)');
plot3(xc,yc,zc,'ro','MarkerFaceColor','k','MarkerSize',10);
%--------------------------------------------------------%
% 2. PARMETROS MECNICOS DEL ROBOT
%--------------------------------------------------------%
L1=30;
L2=17;
L3=13;
L4=13;
%while(1)
%--------------------------------------------------------%
% 3. CINEMTICA INVERSA
%------------------------------- -------------------------%
% 3.1. NGULO "theta1"
q1=atan2(yc,xc)-atan2(L2,sqrt(xc^2+yc^2-L2^2));
%------------------------------- -------------------------%
% 3.1. NGULO "theta2"
r=sqrt(xc^2+yc^2-L2^2);
s=L1-zc;
%q2=atan2(s,r)-asin(L3*sin(q3)/sqrt(s^2+r^2));
% 3.2. NGULO "theta3"

num=r^2+s^2-L4^2-L3^2;%L2 X L4
den=2*L4*L3;%L2XL4
D=num/den;%acos(D)=q3>1

if (D>1)
error('POSICIN IMPOSIBLE DE ALCANZAR');
end
q3=acos(D);%q3=atan2(+sqrt(1-D^2),D);
%theta2
q2=atan2(s,r)-asin(L4*sin(q3)/sqrt(s^2+r^2));%L3XL4

%Mensajes
disp('NGULOS');
disp([q1 q2 q3]);
disp([q1 q2 q3]*180/pi);

%--------------------------------------------------------%
% 4. SISTEMA COORDENADO 1
%--------------------------------------------------------%
% 4.1. DEFINCIN DE PARMETROS D-H
theta1=0;%theta1
d1=L1;%Fijo
a1=0;%Fijo
alpha1=-pi/2;
%Fijo%cambiamos esto era pi/2 x 0
% 4.2. DEFINICIN DE MATRIZ HOMOGNEA 'A1'
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
% 4.3. PLOTEAMOS EL SISTEMA COORDENADO
T01=A1;
h1=plot_frame(T01,'frame','1','color','b');
%-------------------------------------------------------%
% 5. SISTEMA COORDENADO DEL ESLABN 2
%--------------------------------------------------------%
% 5.1. DEFINICIN DE PARMETROS D-H
theta2=0 ;%0 deinido
d2=L2;
a2=0;%Fijo%cqambiamos L2 x 0
alpha2=0;%Fjo
% 5.2. DEFINICIN DE LA MATRIZ HOMOGNEA 'A2'
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
% 5.3 PLOTEAMOS EL SISTEMA COORDENADO
T02=T01*A2;
h2=plot_frame(T02,'frame','2','color','r');
%--------------------------------------------------------%
% 6. SISTEMA COORDENADO 3
%--------------------------------------------------------%
% 6.1. DEFINICIN DE PARMETROS D-H
theta3=0;%theta2
d3=0;%Fijo%d3 falta
a3=L3;%Fijo
alpha3=0;%Fjo
% 6.2. DEFINICIN DE LA MATRIZ HOMOGNEA 'A3'
A3=matriz_homogenea_DH(theta3,d3,a3,alpha3);
% 6.3 PLOTEAMOS EL SISTEMA COORDENADO
T03=A1*A2*A3;
h3=plot_frame(T03,'frame','3','color','g');
%%%
%--------------------------------------------------------%
% 6.4. SISTEMA COORDENADO 4
%--------------------------------------------------------%
% 6.5. DEFINICIN DE PARMETROS D-H
theta4=0;%theta3
d4=0;%Fijo%d3 falta
a4=L4;%Fijo
alpha4=0;%Fjo
% 6.2. DEFINICIN DE LA MATRIZ HOMOGNEA 'A3'
A4=matriz_homogenea_DH(theta4,d4,a4,alpha4);
% 6.3 PLOTEAMOS EL SISTEMA COORDENADO
T04=A1*A2*A3*A4;
h4=plot_frame(T04,'frame','4','color','Y');
%--------------------------------------------------------%
% 7. LAZO DE SIMULACIN
%--------------------------------------------------------%
% 7.1. MENSAJES
disp('ROBOT DEL TIPO RR DE 2GDL');
disp('Presione una tecla para empezar la simulacin');
pause;
% 7.2. LAZO PRINCIPAL
STEPS=30;
THETA1=linspace(theta1,q1,STEPS);
THETA2=linspace(theta2,0,STEPS);
THETA3=linspace(theta3,q2,STEPS);
THETA4=linspace(theta4,q3,STEPS);
for i=1:STEPS
% A. OBTNEMOS LOS NGULOS
theta1=THETA1(i);
theta2=THETA2(i);
theta3=THETA3(i);
theta4=THETA4(i);
% B. ACTUALIZAMOS LAS CINMETICA
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
A3=matriz_homogenea_DH(theta3,d3,a3,alpha3);
A4=matriz_homogenea_DH(theta4,d4,a4,alpha4);
T01=A1;
T02=A1*A2;
T03=T02*A3;
 T04=T03*A4;
% C. PLOTEAMOS LOS SISTEMAS COORDENADOS
plot_frame(h1,T01);
plot_frame(h2,T02);
plot_frame(h3,T03);
plot_frame(h4,T04);
%FUNCION FWRITE
fwrite(s1, [q1 q2 q3], 'uchar');
% D. DELTA DE TIEMPO
pause(0.3);
end
%fwrite(s1, [q1 q2 q3 ], 'uchar');
%fwrite(s1, [theta1 theta3 theta4], 'uchar');
disp('Presione una tecla para TERMINAR la simulacin');
pause;
% Mandamos datos a la tarjeta Arduino
%fwrite(s1, [theta1 theta3 theta4], 'uchar');
%fwrite(s1, [q1 q2 q3 ], 'uchar');
end
%fwrite(s1, [theta1 theta2 theta3 theta4], 'uchar');
fclose(s1);