% Leitura de um parametro sensorial de um SCU
%
% [sensors,state,rx,error,errorstr]=readjoint(H,scu_id,param)
%
% Entradas:
% H        => Handler para comunicar com o Master
% scu_id   => Identificador do SCU alvo
% param    => Parametro a ler
%
% Saídas:
% sensors  => Parametros sensoriais
% state    => Bits de estado dos servos
% rx       => Mensagem de baixo nível recebida
% error    => Código de erro, se existente
% errorstr => String descritiva do erro
%
% (c) Milton Ruas da Silva, 2006

function [servos,state,rx,error,errorstr]=readjoint(H,scu_id,param)

% Carregamento em memória das constantes
data;

% Construção da mensagem a enviar
message=buildmessage(OP_READ_SENSORS,scu_id,param);

% Envio da mensagem de request e leitura da resposta
[rx,error,errorstr]=sendmessage(H,message);

% O array servos indica o parametro pedido para cada um dos servos
servos=[];
state=[];

if (~error)
    % Estado do sistema
    for i=7:-1:0
         state=[state bitand(bitshift(rx(end),-i),1)];
    end

    servos=intsigned(double(rx(3:end-1)),8);
    % Conversão para inteiro sem sinal, caso seja corrente o parametro
    if (param==PARAM_CURRENT), servos = intunsigned(servos,8); end
end
