[rx,error,errorstr,tries]=applyjoint(H, 2, 3, [1 1 0])
[rx,error,errorstr,tries]=applyjoint(H, 3, 3, [1 1 0])
applyjoint(H,2,1,[50 50 50]);
applyjoint(H,3,1,[50 50 50]);
applyjoint(H,3,0,[-30 0 0]);
applyjoint(H,2,0,[0 0 0]);

error=1;
while(error)
    [pos_ref,state,rx,error,errorstr,tries] = readjoint(H,2,0);
end

applycontrol(H,2,0,[3 3 3]);
applycontrol(H,2,3,[1 1 1]);

t1 = 50;    % teta1
t2 = -2*t1;    % teta2
t3 = -t2-t1;  % teta3 
t1 = -t1;


teta = [t1, t2, t3]-pos_ref

applyjoint(H,2,0,teta)

t=1
while(t<10)
    error=1;
    while(error)
        [pos_real,state,rx,error,errorstr,tries] = readjoint(H,2,0);
    end
    state0(t,:) = state;
    aux = pos_real-pos_ref;
    aux(1) = -aux(1);
    [j1(t,:) j2(t,:) j3(t,:)] = direct_cine_3R_2(aux(1), aux(2), aux(3), 9.5, 9.5, 6.8);
    subplot(3,1,1);
    plot(j1(1:t,1), j1(1:t, 2), 'r');
    axis auto;
    grid on
    hold on
    subplot(3, 1, 2);
    plot(j2(1:t,1), j2(1:t, 2), 'g');
    axis auto;
    grid on
    subplot(3,1,3);
    plot(j3(1:t,1), j3(1:t, 2), 'b');
    axis auto;
    grid on
    hold off
    
    pause(1/20);
    t=t+1;
end

pause(1)


applyjoint(H,2,0,pos_ref)
applyjoint(H,2,0,off_set_juntas)



% 0.016 = 1/60
% tic
% error=1;
% while(error)
%     [pos_ref,state,rx,error,errorstr,tries] = readjoint(H,2,0);
% end
% toc
% pos_ref