function [qtilde tau] = cap8_PIDrobot2gdl(x, xp, varepsilon)
q = [x(1); x(2)];   %posiciones
qp = [xp(1); xp(2)]; %velocidades
kp1=2.666; kp2=0.133;
Kp=[kp1, 0; 0, kp2];  %ganancia proporcional
ki1=0.1*kp1;
ki2=0.1*kp2;
Ki=[ki1, 0; 0, ki2];  %ganancia integral
kv1=0.2*kp1;
kv2=0.2*kp2;
Kv=[kv1,  0; 0, kv2]; %ganancia derivativa
%referencias
qd1=45; qd2=90;
qd=[qd1; qd2];  %vector de posiciones deseadas

%vector de pares de gravitacionales
par_grav = [38.46*sin(q(1))+1.82*sin(q(1)+q(2));
               1.82*sin(q(1)+q(2))       ];

qtilde=[pi*qd(1)/180-q(1); pi*qd(2)/180-q(2)];
%error de posición en grados
qtgrados=(180/pi)*[qtilde(1) ;qtilde(2)]; 
  %velocidad en grados/segundo.
  qpgrados=180*qp/pi;
 tau=Kp*qtgrados+Ki*varepsilon-Kv*qpgrados+par_grav;
end