% Código para robot raplim

close all; clear; clc;

load('raplim_estructura.mat');
q = [0;0;0;0];
L1A = 85;
L1B = 50.5;
L1 = L1A+L1B;
L2 = 120.23;
L3 = 94.6;
L4 = 65;
L = [L1 ; L2; L3; L4];
Le = 100;

thetaDH = [ q(1)+pi/2; q(2); q(3); q(4)];

dDH = [ L(1); 0; 0; 0];
aDH = [ 0; L(2); L(3); L(4)];
alphaDH = [ pi/2; 0; 0; 0];

Am0 = eye(4);
A01 = denavit(thetaDH(1), dDH(1), aDH(1), alphaDH(1));
A12 = denavit(thetaDH(2), dDH(2), aDH(2), alphaDH(2));
A23 = denavit(thetaDH(3), dDH(3), aDH(3), alphaDH(3));
A34 = denavit(thetaDH(4), dDH(4), aDH(4), alphaDH(4));

Am1 = Am0*A01;
Am2 = Am1*A12;
Am3 = Am2*A23;
Am4 = Am3*A34;

dibujar_objeto_matlab_from_stl(raplim.stl.eslabon0,Am0);
dibujar_objeto_matlab_from_stl(raplim.stl.eslabon1,Am1);
camlight(40,20);,lighting phong;

dibujarSistemaReferenciaMTH(Am0, Le, 5, '0');

dibujarSistemaReferenciaMTH(Am1, Le, 5, '1');
dibujarSistemaReferenciaMTH(Am2, Le, 5, '2');
dibujarSistemaReferenciaMTH(Am3, Le, 5, '3');
dibujarSistemaReferenciaMTH(Am4, Le, 5, '4');

grid on;
%axis([-10,10,-10,10,-10,10]);
view(100,20);