/// <summary>
/// Set positions of specified servos.
/// </summary>
///
/// <param name="mask">Mask array specifying which servos need to be set.</param>
/// <param name="positions">Array of servos' positions to set. Each position is in [0, 255] range.</param>
///
/// <returns>Returns array of current servos' positions.</returns>
///
/// <remarks><para>The <paramref name="mask"/> and <paramref name="positions"/> arrays specify
/// which Qwerk's servo's state should be updated. If value of the <paramref name="mask"/>
/// array is set to <see langword="true"/>, then corresponding servo's state is changed to the state,
/// which is specified in <paramref name="positions"/> array.</para>
/// </remarks>
///
/// <exception cref="ArgumentException">Incorrect length of <paramref name="mask"/> or
/// <paramref name="positions"/> array.</exception>
/// <exception cref="NotConnectedException">No connection to Qwerk or its service.</exception>
/// <exception cref="ConnectionLostException">Connestion to Qwerk is lost.</exception>
///
public int[] SetPositions(bool[] mask, int[] positions)
{
if ((mask.Length != Count) || (positions.Length != Count))
{
throw new ArgumentException("Incorrect length of mask or positions array.");
}
// check controller
if (servoController == null)
{
throw new NotConnectedException("Qwerk's service is not connected.");
}
try
{
TeRKIceLib.ServoCommand command = CreateCommand( );
for (int i = 0; i < Count; i++)
{
command.servoMask[i] = mask[i];
command.servoModes[i] = TeRKIceLib.ServoMode.ServoMotorPositionControl;
command.servoPositions[i] = positions[i];
command.servoSpeeds[i] = DefaultSpeed;
}
// execute servos' command
TeRKIceLib.ServoState state = servoController.execute(command);
return(state.servoPositions);
}
catch
{
throw new ConnectionLostException("Connection is lost.");
}
}
// Create servos' command
private TeRKIceLib.ServoCommand CreateCommand( )
{
TeRKIceLib.ServoCommand command = new TeRKIceLib.ServoCommand( );
command.servoMask = new bool[Count];
command.servoModes = new TeRKIceLib.ServoMode[Count];
command.servoPositions = new int[Count];
command.servoSpeeds = new int[Count];
return(command);
}
// Create servos' command
private TeRKIceLib.ServoCommand CreateCommand( )
{
TeRKIceLib.ServoCommand command = new TeRKIceLib.ServoCommand( );
command.servoMask = new bool[Count];
command.servoModes = new TeRKIceLib.ServoMode[Count];
command.servoPositions = new int[Count];
command.servoSpeeds = new int[Count];
return command;
}
