%Programa que genera los movimientos de un robot de 2 grados de libertad
%23 Marzo 2012
clear; close
%Longitud de los eslabones y ángulos
l1=2; l2=2;
q1=0; q2=0;
P1=[0,0,0,1]';
P2=[l1,0,0,1]';
P3=[l1+l2,0,0,1]';
x1=[P1(1),P2(1)]; y1=[P1(2),P2(2)]; z1=[P1(3),P2(3)];
x2=[P2(1),P3(1)]; y2=[P2(2),P3(2)]; z2=[P2(3),P3(3)];
R1=line(x1, y1, z1, 'Color','r','LineWidth',4)
R2=line(x2, y2, z2, 'Color','g','LineWidth',4)
axis([-4,4,-4,4])
grid
hold
pause

i=1;
for q1=0:pi/10:2*pi
    for q2=0:pi/8:2*pi
        P1=[0,0,0,1]';
        P2=rotz(q1)*[l1,0,0,1]';
        P3=rotz(q1)*transl([l2,0,0])*rotz(q2)*[l1,0,0,1]';
        x1=[P1(1),P2(1)]; y1=[P1(2),P2(2)]; z1=[P1(3),P2(3)];
        x2=[P2(1),P3(1)]; y2=[P2(2),P3(2)]; z2=[P2(3),P3(3)];
        set(R1,'xdata',x1,'ydata',y1,'zdata',z1);
        set(R2,'xdata',x2,'ydata',y2,'zdata',z2);
        X(i)=P3(1); Y(i)=P3(2); Z(i)=P3(3);
        plot(X,Y)
        i=i+1;
        pause(0.1)
    end
end

        