// Create motors' command
private TeRKIceLib.MotorCommand CreateCommand( )
{
TeRKIceLib.MotorCommand command = new TeRKIceLib.MotorCommand( );
command.motorMask = new bool[Count];
command.motorModes = new TeRKIceLib.MotorMode[Count];
command.motorPositions = new int[Count];
command.motorVelocities = new int[Count];
command.motorAccelerations = new int[Count];
return(command);
}
/// <summary>
/// Stop all motors.
/// </summary>
///
/// <exception cref="NotConnectedException">No connection to Qwerk or its service.</exception>
/// <exception cref="ConnectionLostException">Connestion to Qwerk is lost.</exception>
///
public void StopMotors( )
{
// prepare command
TeRKIceLib.MotorCommand command = CreateCommand( );
for (int i = 0; i < Count; i++)
{
command.motorMask[i] = true;
command.motorModes[i] = TeRKIceLib.MotorMode.MotorOff;
}
ExecuteCommand(command);
}
/// <summary>
/// Stop specified motor.
/// </summary>
///
/// <param name="motor">Motor to stop, [0, <see cref="Motors.Count"/>).</param>
///
/// <exception cref="ArgumentOutOfRangeException">Invalid motor is specified.</exception>
/// <exception cref="NotConnectedException">No connection to Qwerk or its service.</exception>
/// <exception cref="ConnectionLostException">Connestion to Qwerk is lost.</exception>
///
public void StopMotor(int motor)
{
if ((motor < 0) || (motor >= Count))
{
throw new ArgumentOutOfRangeException("Invalid motor is specified.");
}
// prepare command
TeRKIceLib.MotorCommand command = CreateCommand( );
command.motorMask[motor] = true;
command.motorModes[motor] = TeRKIceLib.MotorMode.MotorOff;
ExecuteCommand(command);
}
/// <summary>
/// Set velocity of specified motor.
/// </summary>
///
/// <param name="motor">Motor to set velocity for, [0, <see cref="Motors.Count"/>).</param>
/// <param name="velocity">Velocity to set.</param>
///
/// <remarks><para>The method sets specified motor's velocity, which is measured in
/// ticks per second. "Ticks" is a made-up term, which does not depend on specific motor,
/// but is an unknown in distance and rotation (see Qwerk documentation for details).</para></remarks>
///
/// <exception cref="ArgumentOutOfRangeException">Invalid motor is specified.</exception>
/// <exception cref="NotConnectedException">No connection to Qwerk or its service.</exception>
/// <exception cref="ConnectionLostException">Connestion to Qwerk is lost.</exception>
///
public void SetMotorVelocity(int motor, int velocity)
{
if ((motor < 0) || (motor >= Count))
{
throw new ArgumentOutOfRangeException("Invalid motor is specified.");
}
// prepare command
TeRKIceLib.MotorCommand command = CreateCommand( );
command.motorMask[motor] = true;
command.motorModes[motor] = TeRKIceLib.MotorMode.MotorSpeedControl;
command.motorVelocities[motor] = velocity;
command.motorAccelerations[motor] = DefaultAcceleration;
ExecuteCommand(command);
}
// Execute motors' command
private void ExecuteCommand(TeRKIceLib.MotorCommand command)
{
// check controller
if (motorController == null)
{
throw new NotConnectedException("Qwerk's service is not connected.");
}
try
{
// execute motors' command
motorController.execute(command);
}
catch
{
throw new ConnectionLostException("Connection is lost.");
}
}
