% 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)

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

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

% Correção das posições solicitadas
position_final=position+calib(scuid,:);
