% Adaptação dos angulos solicitados ao que os motores requerem
%
% pos_final=adaptate(scuid,position)
%
% scuid    -> ID do slave
% position -> Posição solicitada (para os tres servos)
% pos_final-> Posição a aplicar aos servos
%
% Note que a correcção nem a limitação é executada.

function pos_final=adaptate(scuid,position,Nrel,servo)

% Preenchimento de algum argumento não fornecido
if (nargin<4)
    servo=1;
end
if (nargin<3)
    Nrel=1;
end

% Sinal a adicionar as posições solicitadas
signal=[-1 +1 +1;
        +1 +1 -1;
        -1 +1 +1;
        -1 +1 +1];

% Offset a adicionar as posições solicitadas
offset=[   0   0 -70;
           0   0 +70;
           0   0   0;
           0   0   0];

% Validação dos argumentos
if (scuid<1 || size(position,2)>min(size(signal,2),size(offset,2)))
    error('limit fcn:invalid arg','Argumentos inválidos!');
end
if (scuid>min(size(signal,1),size(offset,1)))
    pos_final=position;
    return;
end

N=size(position,1);
pos_final=zeros(N,3);

for i=1:N
    if (servo>0)
        % Adapatção da posição solicitada para aplicação aos motores
        pos_final(i,:) = position(i,:).*signal(scuid,:)*Nrel + offset(scuid,:);
    else
        % Adaptação da posição lido nos motores para a das juntas
        pos_final(i,:) = (position(i,:)-offset(scuid,:)).*signal(scuid,:)/Nrel;
    end
end