function [t0, t1, t2, t3] = jacontrol2(ex, ey, k1, k2)

k = diag([k1, k2]);

e = [ex ey]';

J = [   0    0   0   1
        -1   0   0   0 ];
    
Jt = eye(4)/J;

teta = Jt*k*e;
t0 = teta(1)*2.5;
t1 = teta(2)*2.5;
t2 = teta(3)*2.5;
t3 = teta(4)*2.5;



% function [t0, t1, t2] = jacontrol(t0, t1, t2, ex, ey, k1, k2, k3)
% 
% M1 = 45;
% M2 = M1;
% R1 = 47;
% R2 = R1;
% L1 = 95;
% L2 = L1;
% 
% mT = M1+M2;
% k = diag([k1, k2, k3]);
% 
% t1 = pi/2-t1;
% 
% s2 = M2*R2*sin(t1+t2)/mT;
% c2 = M2*R2*cos(t1+t2)/mT;
% 
% s12 = (M1*R1+M2*L1)*sin(t1)/mT + s2;
% c12 = (M1*R1+M2*L1)*cos(t1)/mT + c2;
% 
% Jt = [  s12*cos(t0)     0      -s12*sin(t0)
%         c12*sin(t0)    -s12     c12*cos(t0)
%         c2*sin(t0)     -s2      c2*cos(t0)  ];
%     
% w = Jt*k*[ex;ey;0];
% 
% w0 = w(1,:);
% w1 = w(2,:);
% w2 = w(3,:);
% 
% t0 = t0+w0*0.5; t0 = mod(t0, sign(t0)*pi/2);
% t1 = t1+w1*0.5; t1 = pi/2-t1; t1 = mod(t1, sign(t1)*pi/2);
% t2 = t2+w2*0.5; t2 = mod(t2, sign(t2)*pi/2);