% Limitação das posições a aplicar a determinado SCU
%
% position_final=limit(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 limitada a aplicar às juntas

function [position,error,errorstr]=limit(scuid,position)

error=0;
errorstr='OK';

% Posições limite a aplicar a cada SCU
posmin=[-90 -90 -90;
        -90 -90 -90;
        -90 -90 -90;
        -90 -90 -90];

posmax=[ 90  90  90;
         90  90  90;
         90  90  90;
         90  90  90];

% Validação dos argumentos
if (scuid<0 || scuid>min(size(posmin,1),size(posmax,1)))
    error('limit fcn:invalid arg','Argumentos inválidos!');
end

% Limitação das posições solicitadas
for i=1:size(posmin,2)
    if (position(i)<posmin(scuid,i))
        position(i)=posmin(scuid,i);
        error=1;
    end
end
for i=1:size(posmax,2)
    if (position(i)>posmax(scuid,i))
        position(i)=posmax(scuid,i);
        error=1;
    end
end

if (error==1)
    errorstr='Posições limitadas!';
end