%   Trajectória da junta da anca
%
%   Data: 23-01-2007
%


l1 = 0;
l2 = 11.5;
l3 = 16;

servos = readjoint(H, 2, 0);
pi1 = servos(1);
pi2 = servos(2);
pi3 = servos(3);


figure();
hold on
tic;
while(1),
    servos = [];
    pause(1/8);
    while(isempty(servos)),
        servos = readjoint(H, 2, 0);
    end
    
    [r1, r2, r3] = direct_cine_3R(deg2rad(servos(1)-pi1), deg2rad(servos(2)-pi2), deg2rad(servos(3)-pi3), l1, l2, l3);
    r3
    plot(toc, r3(1), 'kx');
    axis auto;
    %drawnow %pause
end