/// <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; }