% 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=adapt(scuid,position,Nrel,servo)
% mlock


persistent calib


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

% Desvios de posição de cada unidade de controlo
if(isempty(calib))
    if(exist('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'file'))
        load('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
    else
        calib=[0   0   0;       % SCU 1: Perna direita  (tornozelo + pé + joelho)
               0   0   0;       % SCU 2: Perna esquerda (tornozelo + pé + joelho)
               0   0   0;       % SCU 3: Anca direita   (mov. dianteiro + lateral + rotativo)
               0   0   0];      % SCU 4: Anca esquerdo  (mov. dianteiro + lateral + rotativo)
        save('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
    end
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];
       
if(nargin == 1)
    H = evalin('base', 'H');
    calib(scuid,:) = readjoint(H,scuid,0)-offset(scuid,:)
    save('c:/mATLAB7/toolbox/comm2/matriz_calib.mat', 'calib');
    disp('matriz de calibração ajustada com sucesso')
    return
end

% 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(i) + offset(scuid,:)+calib(scuid,:);
    else
        % Adaptação da posição lido nos motores para a das juntas
        pos_final(i,:) = (position(i,:)-offset(scuid,:)).*signal(scuid,:)/Nrel-calib(scuid,:);
    end
end