%-----------------------------------------------------------------------+
% EnviaOrdem(Porta, Mensagem, Ordem)                                    |
%                                                                       |
% Autor:   David Manuel Costa Gameiro                                   |
%                                                                       |
% Date:    13 / Julho / 2004                                            |
%                                                                       |
% Funçao:  Enviar uma ordem ao Robot, atraves da porta serie            |
%                                                                       |
% Input:   Porta - Porta para comando do gripper-->'COM1'ou'COM2'ou ... |
%          Mensagem - vector com a seguinte ordem,                      |
%          [Lado;  qual o lado do robot, (0)-esquerdo, (1)-direito      |
%          Membro; qual o Membro a usar (0)-Perna, (1)-Braco            |
%          NumJunta; qual a junta do membro                             |
%          Ordem] escrever uma nova posicao para essa junta             |
%                                                                       |
%          Ordem:                                                       |
%             'home'|(0), coloca o robot na sua Home Position           |
%             'NovPos'|(1), escreve a nova posicao para a junta         |
%             'LePos'|(2), Obtem posicao actual de determinada junta    |
%             'LeMembro'|(3), Obtem posicao actual de determinada junta |
%             ''|(4), |
%             ''|(5), |
%                                                                       |
% Output:  Porta - Objecto que contem todas as propriedades da porta    |
%         serie usada no controlo do PHUA                               |
%                                                                       |
%-----------------------------------------------------------------------+

function EnviaOrdem(Porta, Mensagem, Ordem)

Byte1 = [dec2bin(Mensagem(1)), dec2bin(Mensagem(2)), MaisZeros(dec2bin(Mensagem(3)), 3)];

switch (Ordem)
    %escrita de mensagens
    case 0%coloca o robot na homeposition
        Byte1 = ['001', Byte1];% primeiro char com a ordem
        Byte2 = 255;% segundo byte
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');
    case 1%escreve a nova posiçao
        Byte1 = ['001', Byte1]; Byte2 = Mensagem(4);
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');            
    %Leitura de dados
    case 2%sensor d tda a junta
        Byte1 = ['010', Byte1];Byte2 = Mensagem(4);
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');
    case 3%sensores d tdo o membro
        Byte1 = ['010', Byte1]; Byte2 = 255;
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');
    case 4%Posiçao actual do servo
        Byte1 = ['011', Byte1]; Byte2 = Mensagem(4);
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');
    case 5%Sensores Força Pe
        Byte1 = ['100', Byte1]; Byte2 = Mensagem(4);
        fwrite(Porta,[bin2dec(Byte1), Byte2],'uchar');
        fread(Porta);
end%termina o switch

%indica a mensagem em binario a enviar 

return
end%termina a funcao