%practico de control dinamico


clc
close all
clear all
instrreset

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

% Cinematica inversa
%implementada en funcion CinInversa

%% CINEMATICA DIRECTA

close all;

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', 'paletizador');
bot.n;

% Finally we can draw a stick figure of our robot
 ws = [-1 1, -1 1, -1 1]*300;
t=0:0.025:1;

% %trayectoria horizontal 

% q1 = interp1([0 1],[-100 -50],t);
% q2 = interp1([0 1],[-50 0],t);
% q0=[q1 q2];
% for i=1:length(q0)
%  Q0(i,:)=CinInversa(q0(i),100,39,1,0);
% end

%trayectoria vertical
q2 = CinInversa(0,230,250,0,1);
q3 = CinInversa(0,230,100,0,1);
Q1 = jtraj(q2,q3,t);

% 
% for i=1:length(Q0)
%    atj=bot.fkine(Q0(i,:));
%    jta=transpose(atj);
%    JTA(i,:)=jta(4,1:3);
% end
% plot3(JTA(:,1),JTA(:,2),JTA(:,3),'.r');
% % bot.plot(q0);
% bot.plot(Q0,'view','top');

% bot.teach;

%  art4=q(:,4);
% art4=art4*180/pi-5;

q=Q1;

% aca habria que cargar la parte de arduino
    arduin=serial('COM6','BaudRate',115200); % create serial communication object on port arduino
    fopen(arduin);
    
    for i= 1:3
    
    art1=q(i,1);
    art2=q(i,2);
    art3=q(i,3);
    art4=q(i,4);
    art5=q(i,5);
    
    
    msg=[': ',num2str(round((art1*180/pi))),',',num2str(round((art2*180/pi))),',',num2str(round((((art3*180/pi))+90))),',',num2str(round(((art4*180/pi)-90)*-1)),',',num2str(round((art5*180/pi)))];
    fprintf(arduin,'%s\r',msg);
    fprintf('%s\r',msg);
    pause(0.5);
end