%% Generacion de Treyectorias para Hexapod
%Proyecto 1 de Robotica 1
%Por 
%Gonzalo Palarea 15559
%Rodrigo Figueroa 161678
%Catedratico:
%Miguel Zea

%% Paso 1: Definir parametros de una sola pierna
%q0 = [pi,pi/2]; %angulo inicial
q0 = [0,0];
theta1 = q0(1); %rotacion en Z de junta 1 a junta 2 en radianes
phi1 = pi/2; %rotacion en X de junta 1 a junta 2 en radianes
r1 = -1.6885; %distancia de junta 1 a junta 2 en z en cm
a1 = 5.0919; %distancia de junta 1 a junta 2 en x en cm
theta2 = q0(2); %rotacion en Z de junta 2 a punta de pata en radianes
phi2 = 0; %rotacion en X de junta 2 a punta de pata en radianes
a2 = 6.8135; %distancia de junta 2 a punta de pata en cm
r2 = -1.3461; %distancia de junta 2 a punta de pata en z en cm

%Matriz_DH = [ theta1, r1, a1, phi1;
%              theta2, r2, a2, phi2];
A1 = genDHmatrix(q0(1), r1,a1,phi1); 
A2 = genDHmatrix(q0(2), r2,a2,phi2);
Matriz_DH_V2 = A1*A2; %Encontramos la matriz de DH
%% Cinematica directa

%%Definir transformaciones de base para acomodar cada pierna en su lugar
%%correcto. Suponiendo que el marco de referencia inercial se encuentra en
%%el centro del Hexapod

%Transformaciones de Base de cada pierna
T_B_Pierna_1 = trotz(-45)*transl(4.7445,-7.3823, 2.4870);
T_B_Pierna_2 = trotz(0)*transl(4.7445,0, 2.4870);
T_B_Pierna_3 = trotz(45)*transl(4.7445,7.3823, 2.4870);
T_B_Pierna_4 = trotz(135)*transl(-4.7445,7.3823, 2.4870);
T_B_Pierna_5 = trotz(180)*transl(-4.7445,0, 2.4870);
T_B_Pierna_6 = trotz(225)*transl(-4.7445,-7.3823, 2.4870);

%Cinematica directa de cada pierna
F_K_Pierna_Sin_Transformar = getFK(Matriz_DH_V2);
F_K_Pierna_1 = getFK(T_B_Pierna_1*Matriz_DH_V2);
F_K_Pierna_2 = getFK(T_B_Pierna_2*Matriz_DH_V2);
F_K_Pierna_3 = getFK(T_B_Pierna_3*Matriz_DH_V2);
F_K_Pierna_4 = getFK(T_B_Pierna_4*Matriz_DH_V2);
F_K_Pierna_5 = getFK(T_B_Pierna_5*Matriz_DH_V2);
F_K_Pierna_6 = getFK(T_B_Pierna_6*Matriz_DH_V2);

%% haciendo Pruebas con el toolbox
L1 = Revolute('d',r1,'a',a1,'alpha',phi1,'offset',0);
L2 = Revolute('d',r2,'a',a2,'alpha',phi2,'offset',0);
Unapierna = SerialLink([L1,L2],'name', 'Pierna_Hexapod');
pata1=SerialLink([L1,L2],'name','pata1','base',SE3(4.7445,-7.3823, 2.4870)*SE3.Rz(-pi/4));
pata2=SerialLink([L1,L2],'name','pata2','base',SE3(4.7445,0, 2.4870)*SE3.Rz(0));
pata3=SerialLink([L1,L2],'name','pata3','base',SE3(4.7445,7.3823, 2.4870)*SE3.Rz(pi/4));
pata4=SerialLink([L1,L2],'name','pata4','base',SE3(-4.7445,7.3823, 2.4870)*SE3.Rz(1*(pi-pi/4)));
pata5=SerialLink([L1,L2],'name','pata5','base',SE3(-4.7445,0, 2.4870)*SE3.Rz(pi));
pata6=SerialLink([L1,L2],'name','pata6','base',SE3(-4.7445,-7.3823, 2.4870)*SE3.Rz(pi+pi/4));
%patas(1).fkine(q0,'deg');
pata1.plot(q0);
hold on;
pata2.plot(q0);
 pata3.plot(q0);
 pata1.teach
 pata2.teach()
 pata3.teach()%  pata4.plot(q0);
%  pata5.plot(q0);
%  pata6.plot(q0);
% patas(4).teach();
%% Cinematica Diferencial (solo de posicion)

%Primero debemos de extraer la posicion del efector final
q1_J=0;
q2_J=0;
%p(q1_J,q2_J)= F_K_Pierna_Sin_Transformar(1:2,3);
% Usando el Toolbox
Jacobiano_Pata_1=pata1.jacob0(q0);
Jacobiano_Pata_2=pata2.jacob0(q0);
Jacobiano_Pata_3=pata3.jacob0(q0);
Jacobiano_Pata_4=pata4.jacob0(q0);
Jacobiano_Pata_5=pata5.jacob0(q0);
Jacobiano_Pata_6=pata6.jacob0(q0);

%usando la funcion creada en clase
Jacobiano = ur5J(q0');
 
%% Cinematica Inversa
I_K_Pierna_1 = ur5IK9(F_K_Pierna_1, q0, 1);
%% Generacion de Trayectorias
%V = [0,0,0;1,0.5,1;2,0,0;0,0,0];
V= [2,0,-1.5;0,0,-1.5;0.5,0.5,2;2,0,-1.5;0,0,-1.5]; %Aqui podes generar la trayectoria basada en una serie de puntos que querras que te recorra
PosInicial = [0;0;0];
Trayectoria1 = mstraj(V, 1, [], PosInicial', 0.1, 1);
tam_trayectoria = length(Trayectoria1);
figure('visible','on');
plot3(PosInicial(1),PosInicial(2),PosInicial(3), 'o');
hold on;
plot3(Trayectoria1(:,1),Trayectoria1(:,2), Trayectoria1(:,3));
Trayectoria_mod_1 = F_K_Pierna_1;
angulos_pierna = zeros([1 tam_trayectoria]);
J1_P1 = zeros([1 tam_trayectoria]);%Creando las matrices para hacer al programa mas rapido
J2_P1 = zeros([1 tam_trayectoria]);
%J1_P2 = zeros([1 tam_trayectoria]);
%J2_P2 = zeros([1 tam_trayectoria]);
%J1_P3 = zeros([1 tam_trayectoria]);
%J2_P3 = zeros([1 tam_trayectoria]);
%J1_P4 = zeros([1 tam_trayectoria]);
%J2_P4 = zeros([1 tam_trayectoria]);
%J1_P5 = zeros([1 tam_trayectoria]);
%J2_P5 = zeros([1 tam_trayectoria]);
%J1_P6 = zeros([1 tam_trayectoria]);
%J2_P6 = zeros([1 tam_trayectoria]);
for n=1:tam_trayectoria 
   F_K_Pierna_1(1:3,4)= Trayectoria1(n,:)'; %Substituimos los puntos de la trayectoria que queremos
   Mod_matrix_1 = ur5IK9( F_K_Pierna_1, q0, 1);%Hacemos cinematica inversa
   J1_P1(n) = Mod_matrix_1(1,1); % Guardamos el valor de el angulo de cada servo en su respectivo array
   J2_P1(n) = Mod_matrix_1(2,2);
end
angulos_deg =rad2deg(angulos_pierna);
J1_P1 = round(rad2deg(J1_P1)); %Los pasamos a grados debido al formato de la libreria 'Servo.h' de Arduino
J2_P1 = round(rad2deg(J2_P1));
Junta_Positiva = abs(J2_P1);
%% Guardar datos en un archivo de texto
 dlmwrite('Movimientos Junta 1 Pata 1.txt',J1_P1) %Guardamos los valores en un archivo de texto para facilitar que se copien y se pueguen
 dlmwrite('Movimientos Junta 2 Pata 1.txt',J2_P1)
 dlmwrite('Movimientos Junta 2 Pata 1.2.txt',Junta_Positiva)
 %dlmwrite('Movimientos Junta 1 Pata 2.txt',J1_P2)
 %dlmwrite('Movimientos Junta 2 Pata 2.txt',J2_P2)
 %dlmwrite('Movimientos Junta 1 Pata 3.txt',J1_P3)
 %dlmwrite('Movimientos Junta 2 Pata 3.txt',J2_P3)
 %dlmwrite('Movimientos Junta 1 Pata 4.txt',J1_P4)
 %dlmwrite('Movimientos Junta 2 Pata 4.txt',J2_P4)
 %dlmwrite('Movimientos Junta 1 Pata 5.txt',J1_P5)
 %dlmwrite('Movimientos Junta 2 Pata 5.txt',J2_P5)
 %dlmwrite('Movimientos Junta 1 Pata 6.txt',J1_P6)
 %dlmwrite('Movimientos Junta 2 Pata 6.txt',J2_P6)