%Entrega Robótica

q = [pi 0 0];
y = [-1.17 0 -7.89];
x = [-1.775 -4.51 0];
alfa = [pi/2 -pi/2 0];
%        theta      y     x       alfa
% M_Har = [ q(1)    -1.17 -1.775      pi/2;
%           q(2)      0   -4.51       -pi/2 ;
%           q(3)    -7.89    0          0  ];
      
q1 = [0 0 0];      
%transl(65.517,y,z)
%trotx("grados")
      
% Verificar matriz DH

% Vector de configuración de las patas.

% Para este lab, alpha se refiere a la rotación respecto a X, en que muchos
% casos, casi siempre va a ser así. El offset, en este caso, se refiere a
% la rotación respecto a Z. De nuevo, en mucho casos, así va a ser. Tomar
% en cuenta que en estas transformaciones, la rotación de alpha es el
% ÚLTIMO!!

L1 = Revolute('d', y(1), 'a', x(1), 'alpha', alfa(1), 'offset', q(1));
L2 = Revolute('d', y(2), 'a', x(2), 'alpha', alfa(2), 'offset', q(2));
L3 = Revolute('d', y(3), 'a', x(3), 'alpha', alfa(3), 'offset', q(3));

Hexapod1 = SerialLink([L1, L2, L3], 'name', 'Pata1', 'base', Hex_Sim(1));
Hexapod2 = SerialLink([L1, L2, L3], 'name', 'Pata2', 'base', Hex_Sim(2));
Hexapod3 = SerialLink([L1, L2, L3], 'name', 'Pata3', 'base', Hex_Sim(3));
Hexapod4 = SerialLink([L1, L2, L3], 'name', 'Pata4', 'base', Hex_Sim(4));
Hexapod5 = SerialLink([L1, L2, L3], 'name', 'Pata5', 'base', Hex_Sim(5));
Hexapod6 = SerialLink([L1, L2, L3], 'name', 'Pata6', 'base', Hex_Sim(6));
% q0 = zeros(3);
% quad.plot(q0, 'zoom', 1.5);
zoom = 2;

figure(1);
Hexapod1.teach(q1, 'zoom', zoom);
hold on
Hexapod2.teach(q1, 'zoom', zoom);
Hexapod3.teach(q1, 'zoom', zoom);
Hexapod4.teach(q1, 'zoom', zoom);
Hexapod5.teach(q1, 'zoom', zoom);
Hexapod6.teach(q1, 'zoom', zoom);