%                   Universidade de Aveiro
%                               -
%       Departamento de Electrónica e Telecomunicações
%
% Projecto 5ºAno - Desenvolviento de Algoritmos de Controle para um Robô
% Humanoide
%
% Pedro Miguel Ferreira     
% Nº Mec - 27593
%
% Data - 07-03-2007
%
%LER_POS   Posição angular da perna das juntas
%   [position,state,rx,error,errorstr,tries] = ler_pos(handler, scu_id, perna),
%   devolve a posição angular das juntas normalizadas segundo a orientação:
%       position(1) - angulo que a junta 1 faz com a superficie
%       position(2) - angulo que a junta 2 faz com a junta 1
%       position(3) - angulo que a junta 3 faz com a junta 2
%
% Entradas:
% handler  => Handler para comunicar com o Master
% scu_id   => Identificador do SCU alvo
% perna    => Perna - 'direita' ou 'esquerda'
% position => Posição angular de cada junta
%
% Saídas:
% rx       => Mensagem de baixo nível recebida
% error    => Código de erro, se existente
% errorstr => String descritiva do erro
% tries    => Número de tentativas para efectuar a comunicação

function [rx,error,errorstr] = apply_pos(handler, scu_id, perna, position)


    if (perna == 'dir')
       position(1) = 90-position(1);
       position(2) = 90-position(2);
       position(3) = position(3)-70/2.5;
       
    elseif(perna == 'esq')
       position(1) = 90-position(1);
       position(2) = position(2);
       position(3) = position(3)+70/2.5;
       
    else
        rx = [0 0 0 0 0 0 0 0];
        error = 1;
        errorstr = 'Perna inválida';
        tries = 0;
        return;
    end
    
    position = position.*[2.5 2.5 2.5];
    
    if(abs(position)>=90)
        disp('Angulo fora da gama');
        return;
    end
    
    % aplica a função com 3 tentativas
    i = 1;
    error = 1;
    while(error)
        [rx,error,errorstr] = applyjoint(handler, scu_id, 0, position);
        i = i+1;
        if(i>3)
            break;
        end
    end