public bool GetMotorState( Motor motor, out MotorState state )
{
state = new MotorState( );
// check motor port
if ( motor == Motor.All )
{
throw new ArgumentException( "Motor state can be retrieved for one motor only" );
}
byte[] command = new byte[3];
byte[] reply = new byte[25];
// prepare message
command[0] = (byte) NXTCommandType.DirectCommand;
command[1] = (byte) NXTDirectCommand.GetOutputState;
command[2] = (byte) motor;
if ( SendCommand( command, reply ) )
{
state.Power = (sbyte) reply[4];
state.Mode = (MotorMode) reply[5];
state.Regulation = (MotorRegulationMode) reply[6];
state.TurnRatio = (sbyte) reply[7];
state.RunState = (MotorRunState) reply[8];
// tacho limit
state.TachoLimit = reply[9] | ( reply[10] << 8 ) |
( reply[11] << 16 ) | ( reply[12] << 24 );
// tacho count
state.TachoCount = reply[13] | ( reply[14] << 8 ) |
( reply[15] << 16 ) | ( reply[16] << 24 );
// block tacho count
state.BlockTachoCount = reply[17] | ( reply[18] << 8 ) |
( reply[19] << 16 ) | ( reply[20] << 24 );
// rotation count
state.RotationCount = reply[21] | ( reply[22] << 8 ) |
( reply[23] << 16 ) | ( reply[24] << 24 );
return true;
}
return false;
}
