public bool SendCommand( byte[] command, byte[] reply )
{
bool result = false;
lock ( sync )
{
// check connection
if ( communicationInterface == null )
{
throw new NullReferenceException( "Not connected to NXT brick" );
}
// send message to NXT brick
if ( communicationInterface.SendMessage( command, command.Length ) )
{
// notifies clients if any
if ( MessageSent != null )
{
MessageSent( this, new CommunicationBufferEventArgs( command ) );
}
if ( ( command[0] == (byte) NXTCommandType.DirectCommandWithoutReply ) ||
( command[1] == (byte) NXTCommandType.SystemCommandWithoutReply ) )
{
result = true;
}
else
{
int bytesRead;
// read message
if ( communicationInterface.ReadMessage( reply, out bytesRead ) )
{
// notifies clients if any
if ( MessageRead != null )
{
MessageRead( this, new CommunicationBufferEventArgs( reply, 0, bytesRead ) );
}
// check that reply corresponds to command
if ( reply[1] != command[1] )
throw new ApplicationException( "Reply does not correspond to command" );
// check for errors
if ( reply[2] != 0 )
{
if ( reply[2] == 221 )
{
throw new ApplicationException( "It seems that a wrong sensor type is connected to the corresponding port" );
}
else
{
throw new ApplicationException( "Error occurred in NXT brick. Error code: " + reply[2].ToString( ) );
}
}
result = true;
}
}
}
}
return result;
}