function TP6_RS_5GDL
clc, clear all, close all
%%% TP6 - Control de Trayectoria de un Robot Serie de 5GDL
COM ='COM13';

instrreset
longHombro = 97;
longBrazo = 142;
longAntBrazo = 77.5;
longMuneca = 71.5;
cabeceo = 0;

arduin=serial(COM,'BaudRate',115200);
% fopen(arduin);
%% Cinemática Directa
L2 = Link([0 longHombro 0 pi/2 0]);
L3 = Link([0 0 longBrazo 0 0]);
L4 = Link([0 0 longAntBrazo 0 0]);
L5 = Link([0 0 0 pi/2 0]);
L6 = Link([0 longMuneca 0 0 0]);
bot = SerialLink([L2 L3 L4 L5 L6], 'name', 'Robot Serie 5 GDL');

%% Posición Inicial
disp('Posición Inicial');
q0 = [pi/2 , pi/2 , pi/2 , pi/2 , pi/2];
% mandar_ard(q0, arduin);
bot.plot(q0,'workspace',[-200 200 -300 300 0 300],'trail',{'.r'}), hold on
hacer_string(q0);

pause

%% Cinemática Inversa
%%% Implementada en función CinInversa

t=0:0.05:3.8;
n=length(t)
%% Trayectoria Horizontal
q0 = CinInversa(-120, 180, 250, 0 ,1);    T0=bot.fkine(q0);
q1 = CinInversa(120, 180, 250, 0 ,1);     T1=bot.fkine(q1);
T12 = ctraj(T0,T1,length(t));           %%% Interpolación Cartesiana para hacer Lineas Rectas
T12=double(T12);                        %%% Esto se hace para poder usar la matriz que devuelve CTRAJ
for i=1:length(t)
    
    Q12(i,:) = CinInversa(T12(1,4,i), T12(2,4,i), T12(3,4,i), 0, 1);
end
qH=real(Q12);
disp("Trayectoria Horizontal");
figure(1)
title("Movimientos Rectos en Plano XY (Horizontal) y Eje Z (Vertical)")
% bot.plot(qH,'workspace',[-200 200 -300 300 0 300],'trail',{'.r'}), hold on
% mandar_ard(qH, arduin);
hacer_string(qH);
pause

%% Trayectoria Vertical
q2 = CinInversa(120,180,250,0,1);        T2=bot.fkine(q2);
q3 = CinInversa(120,180,100,0,1);        T3=bot.fkine(q3);
T23 = ctraj(T2,T3,length(t));           %%% Interpolación Cartesiana para hacer Lineas Rectas
T23=double(T23);                        %%% Esto se hace para poder usar la matriz que devuelve CTRAJ
for i=1:length(t)
    Q23(i,:) = CinInversa(T23(1,4,i), T23(2,4,i), T23(3,4,i), 0, 1);
end
qV=real(Q23);
disp("Trayectoria Vertical");
% bot.plot(qV,'workspace',[-200 200 -300 300 0 300],'trail',{'.r'}), hold off
% mandar_ard(qV, arduin);
hacer_string(qV);
% fclose(arduin);
end

%% Función para Convertir Datos Articulares en Texto
function hacer_string(q)
for i= 1 : length(q(:,1))
    art1=q(i,1);
    art2=q(i,2);
    art3=q(i,3);
    art4=q(i,4);
    
%     msg=['{',num2str(round((art1*180/pi))),',',num2str(round((art2*180/pi))),',',num2str(round((((art3*180/pi))+90))),',',num2str(round(((art4*180/pi)-90)*-1)),'},'];
    msg=['{',num2str(round((art1*180/pi))),',',num2str(round((art2*180/pi-15))),',',num2str(round((((art3*180/pi))-30))),',',num2str(round(((art4*180/pi)+45))),'},'];
    fprintf('%s',msg);
end
fprintf('\n');
end

%% FUNCIÓN PARA ENVIAR DATOS DESDE MATLAB A ARDUINO
function  mandar_ard(q, arduin) 
for i= 1 : length(q(:,1))
    art1=q(i,1);
    art2=q(i,2);
    art3=q(i,3);
    art4=q(i,4);
    
    msg=[':',num2str(round((art1*180/pi))),',',num2str(round((art2*180/pi))),',',num2str(round((((art3*180/pi))+90))),',',num2str(round(((art4*180/pi)-90)*-1))];
    fprintf(arduin,'%s\r',msg);
    fprintf('%s\r',msg);
    pause(0.01);
end
end

function iT=invHomog(T)
iT=eye(4);
iT(1:3, 1:3)= T(1:3, 1:3)';
iT(1:3,4)   = -iT(1:3,1:3)*T(1:3,4);
end

%% FUNCIÓN DE CINEMÁTICA INVERSA (CinInversa)
% function q = CinInversa(pX,pY,pZ,ox,oy)
% if nargin<5
%     error('Faltan datos')
% end
% longHombro = 97;
% longBrazo = 142;
% longAntBrazo = 77.5;
% longMuneca = 71.5;
% cabeceo = 0;
% q2 = atan2(pY,pX);
% Z = pZ-longHombro;
% r=(pX^2+(pY)^2)^(0.5);
% Afr = sin(cabeceo)*longMuneca;
% ladoB = r-Afr;
% Afz = cos(cabeceo)*longMuneca;
% ladoA = Z+Afz;
% hipotenusa = sqrt((ladoA^2)+(ladoB^2));
% alfa = atan2(ladoA,ladoB);
% beta = acos((longBrazo^2 + hipotenusa^2 - longAntBrazo^2) /(2*longBrazo*hipotenusa));
% angBrazo = alfa + beta;
% q3=angBrazo;
% gamma = acos((longBrazo^2+longAntBrazo^2-hipotenusa^2)/(2*longBrazo*longAntBrazo));
% angAntBrazo = -(pi-gamma);
% q4=angAntBrazo;
% angMuneca = cabeceo-angBrazo-angAntBrazo;
% q5=angMuneca;
% q6=acos(-sin(q2)*ox+cos(q2)*oy);
% q = [q2,q3,q4,q5,q6];
% end