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
fprintf('{270,75,60,130},\n');
pause

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

t=0:0.05:3.8;
n=length(t);
%% Trayectoria Horizontal
q0 = [pi/2 , pi/2 , pi/2 , pi/2 , pi/2];    T0=bot.fkine(q0);
q1 = CinInversa(140, -149, 239, 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), -149, 239, 0, 1);
end
qH=(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(140, -149, 239, 0 ,1);        T2=bot.fkine(q2);
q3 = CinInversa(140, -149, 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=(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))),'},'];
%     msg=['{',num2str(round((art1*180/pi))*(-1)),',',num2str(round((art2*180/pi))-15),',',num2str(round((((art3*180/pi))*(-1)))-30),',',num2str(round(((art4*180/pi)))+45),'},'];
    
    msg=['{',num2str(round(180-(art1*180/pi))),',',num2str(round((art2*180/pi))-15),',',num2str(round((((art3*180/pi))+90))+60),',',num2str(round(((art4*180/pi)-90)*-1)+130),'}',','];
    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