% Adaptação dos angulos solicitados ao que os motores requerem
%
% pos_final=adapt(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=adapt(scuid,position,servoact)

% 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];

% Desvios de posição de cada unidade de controlo
calib=[  0   0   0;       % SCU 1: Perna direedit ita  (tornozelo + pé + joelho)
        10  -5   0;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
         0   0   0;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
        -21   0   0];      % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)

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

% Load leg data
footdata;

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