% Converte os angulos das juntas para os dos servos
% Não é considerada a relação de transmissão

function fi=convert2servoangles(theta)

% Relação das correias de transmissão
n=2.5;

% Angulos das juntas
theta1=theta(:,1);
theta2=theta(:,2);

% Angulos dos servos
fi3=theta1+pi/2;
fi2=-theta2;
fi1=fi2-fi3;

% Retorno dos angulos dos servos
fi=[fi1 fi2 fi3]*n;
