% Script de correcção das posições aplicadas nas juntas
%
% position_final=correct(scuid,position)
% Entrada:
%   scuid: id do SCU a aplicar as posições
%   position: lista de posições a aplicar às 3 juntas
% Saída:
%   position_final: posição corrigida a aplicar às juntas

function position_final=correct(scuid,position,servo)

if (nargin<3)
    servo=1;
end

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

% Validação dos argumentos
if (scuid<1 || size(position,2)~=size(calib,2))
    error('correct fcn:invalid arg','Argumentos inválidos!');
end
if (scuid>size(calib,1))
    position_final=position;
    return;
end

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

for i=1:N
    % Correção das posições solicitadas
    if (servo>0)
        position_final(i,:)=position(i,:)+calib(scuid,:);
    else
        position_final(i,:)=position(i,:)-calib(scuid,:);
    end
end