Manipulation of Surveyor SRV-1 Blackfin robot/camera.
The class allows to manipulate with Surveyor SRV-1 Blackfin Robot - getting video from its camera, manipulating motors and servos, reading ultrasonic modules' values, sending direct commands, etc.
Sample usage:
SRV1 srv = new SRV1( ); // connect to SRV-1 robot srv.Connect( "169.254.0.10", 10001 ); // stop motors srv.StopMotors( ); // set video resolution and quality srv.SetQuality( 7 ); srv.SetResolution( SRV1.VideoResolution.Small ); // get version string string version = srv.GetVersion( ); // get robot's camera SRV1Camera camera = srv.GetCamera( ); // set NewFrame event handler camera.NewFrame += new NewFrameEventHandler( video_NewFrame ); // start the video source camera.Start( ); // ... private void video_NewFrame( object sender, NewFrameEventArgs eventArgs ) { // get new frame Bitmap bitmap = eventArgs.Frame; // process the frame } /// <summary>
/// Connect to SVS board.
/// </summary>
///
/// <param name="ipAddress">IP address of SVS board.</param>
///
/// <remarks><para>The method establishes connection to SVS board. If it succeeds then
/// other methods can be used to manipulate the board.</para>
///
/// <para><note>The method calls <see cref="Disconnect"/> before making any connection
/// attempts to make sure previous connection is closed.</note></para>
/// </remarks>
///
/// <exception cref="ConnectionFailedException">Failed connecting to SVS.</exception>
///
public void Connect(string ipAddress)
{
// close previous connection
Disconnect();
lock (sync1)
{
lock (sync2)
{
try
{
communicator1 = new SRV1();
communicator2 = new SRV1();
communicator1.Connect(ipAddress, 10001);
communicator2.Connect(ipAddress, 10002);
hostAddress = ipAddress;
}
catch
{
Disconnect();
throw new ConnectionFailedException("Failed connecting to SVS.");
}
}
}
}
private void DispatchCommand( SRV1.MotorCommand command )
{
if ( SrvDrivingCommand != null )
{
SrvDrivingCommand( this, command );
}
}
/// <summary>
/// Set video resolution.
/// </summary>
///
/// <param name="resolution">Video resolution to set.</param>
///
/// <remarks>
/// <para><note>Setting higher <see cref="SetQuality">quality level</see> and resolution
/// may increase delays for other requests processed by <see cref="SRV1"/> class. So if
/// robot is used not only for video, but also for controlling servos/motors, and higher
/// response level is required, then do not set very high quality and resolution.
/// </note></para>
/// </remarks>
///
public void SetResolution( SRV1.VideoResolution resolution )
{
communicator.SetResolution( resolution );
}
// The class may be instantiate using SVS or SRV1 objects only
internal SRV1Camera( SRV1 communicator )
{
this.communicator = communicator;
}
private void srvDriverControl_SrvDrivingCommand( object sender, SRV1.MotorCommand command )
{
if ( svs.IsConnected )
{
try
{
if ( !receivedFirstDrivingCommand )
{
// use one direct control command first
svs.StopMotors( );
}
// send new command
svs.ControlMotors( command );
if ( ( ( command == SRV1.MotorCommand.DecreaseSpeed ) ||
( command == SRV1.MotorCommand.IncreaseSpeed ) ) &&
( receivedFirstDrivingCommand ) &&
( lastMotorCommand != SRV1.MotorCommand.Stop ) )
{
// resend last command to get effect of speed increase/decrease
svs.ControlMotors( lastMotorCommand );
}
else
{
receivedFirstDrivingCommand = true;
lastMotorCommand = command;
}
}
catch ( Exception ex )
{
System.Diagnostics.Debug.WriteLine( "## " + ex.Message );
}
}
}
// The class may be instantiate using SVS or SRV1 objects only
internal SRV1Camera(SRV1 communicator)
{
this.communicator = communicator;
}
/// <summary>
/// Set video resolution for both cameras.
/// </summary>
///
/// <param name="resolution">Video resolution to set.</param>
///
/// <remarks>
/// <para><note>Setting higher <see cref="SetQuality">quality level</see> and resolution
/// may increase delays for other requests sent to SVS. So if
/// robot is used not only for video, but also for controlling servos/motors, and higher
/// response level is required, then do not set very high quality and resolution.
/// </note></para>
/// </remarks>
///
/// <exception cref="NotConnectedException">Not connected to SVS. Connect to SVS board before using
/// this method.</exception>
///
public void SetResolution( SRV1.VideoResolution resolution )
{
SafeGetCommunicator1( ).SetResolution( resolution );
SafeGetCommunicator2( ).SetResolution( resolution );
}
/// <summary>
/// Control motors connected to SVS board using predefined commands.
/// </summary>
///
/// <param name="command">Motor command to send to the SVS board.</param>
///
/// <remarks><para><note>Controlling SVS motors with this method is only available
/// after at least one direct motor command is sent, which is done using <see cref="StopMotors"/> or
/// <see cref="RunMotors"/> methods.</note></para></remarks>
///
/// <exception cref="NotConnectedException">Not connected to SVS. Connect to SVS board before using
/// this method.</exception>
///
public void ControlMotors( SRV1.MotorCommand command )
{
SafeGetCommunicator1( ).ControlMotors( command );
}
/// <summary>
/// Disconnect from SVS device.
/// </summary>
///
/// <remarks><para>The method disconnects from SVS board making all other methods
/// unavailable (except <see cref="Connect"/> method). In the case if user
/// obtained instance of left or right camera using <see cref="GetCamera"/>
/// method, the video will be stopped automatically (and those <see cref="SRV1Camera"/>
/// instances should be discarded).
/// </para></remarks>
///
public void Disconnect( )
{
lock ( sync1 )
{
lock ( sync2 )
{
hostAddress = null;
// signal cameras to stop
if ( leftCamera != null )
{
leftCamera.SignalToStop( );
}
if ( rightCamera != null )
{
rightCamera.SignalToStop( );
}
// wait until cameras stop or abort them
if ( leftCamera != null )
{
// wait for aroung 250 ms
for ( int i = 0; ( i < 5 ) && ( leftCamera.IsRunning ); i++ )
{
System.Threading.Thread.Sleep( 50 );
}
// abort camera if it can not be stopped
if ( leftCamera.IsRunning )
{
leftCamera.Stop( );
}
leftCamera = null;
}
if ( rightCamera != null )
{
// wait for aroung 250 ms
for ( int i = 0; ( i < 5 ) && ( rightCamera.IsRunning ); i++ )
{
System.Threading.Thread.Sleep( 50 );
}
// abort camera if it can not be stopped
if ( rightCamera.IsRunning )
{
rightCamera.Stop( );
}
rightCamera = null;
}
if ( communicator1 != null )
{
communicator1.Disconnect( );
communicator1 = null;
}
if ( communicator2 != null )
{
communicator2.Disconnect( );
communicator2 = null;
}
}
}
}
/// <summary>
/// Connect to SVS board.
/// </summary>
///
/// <param name="ipAddress">IP address of SVS board.</param>
///
/// <remarks><para>The method establishes connection to SVS board. If it succeeds then
/// other methods can be used to manipulate the board.</para>
///
/// <para><note>The method calls <see cref="Disconnect"/> before making any connection
/// attempts to make sure previous connection is closed.</note></para>
/// </remarks>
///
/// <exception cref="ConnectionFailedException">Failed connecting to SVS.</exception>
///
public void Connect( string ipAddress )
{
// close previous connection
Disconnect( );
lock ( sync1 )
{
lock ( sync2 )
{
try
{
communicator1 = new SRV1( );
communicator2 = new SRV1( );
communicator1.Connect( ipAddress, 10001 );
communicator2.Connect( ipAddress, 10002 );
hostAddress = ipAddress;
}
catch
{
Disconnect( );
throw new ConnectionFailedException( "Failed connecting to SVS." );
}
}
}
}
/// <summary>
/// Disconnect from SVS device.
/// </summary>
///
/// <remarks><para>The method disconnects from SVS board making all other methods
/// unavailable (except <see cref="Connect"/> method). In the case if user
/// obtained instance of left or right camera using <see cref="GetCamera"/>
/// method, the video will be stopped automatically (and those <see cref="SRV1Camera"/>
/// instances should be discarded).
/// </para></remarks>
///
public void Disconnect()
{
lock (sync1)
{
lock (sync2)
{
hostAddress = null;
// signal cameras to stop
if (leftCamera != null)
{
leftCamera.SignalToStop();
}
if (rightCamera != null)
{
rightCamera.SignalToStop();
}
// wait until cameras stop or abort them
if (leftCamera != null)
{
// wait for aroung 250 ms
for (var i = 0; (i < 5) && (leftCamera.IsRunning); i++)
{
System.Threading.Thread.Sleep(50);
}
// abort camera if it can not be stopped
if (leftCamera.IsRunning)
{
leftCamera.Stop();
}
leftCamera = null;
}
if (rightCamera != null)
{
// wait for aroung 250 ms
for (var i = 0; (i < 5) && (rightCamera.IsRunning); i++)
{
System.Threading.Thread.Sleep(50);
}
// abort camera if it can not be stopped
if (rightCamera.IsRunning)
{
rightCamera.Stop();
}
rightCamera = null;
}
if (communicator1 != null)
{
communicator1.Disconnect();
communicator1 = null;
}
if (communicator2 != null)
{
communicator2.Disconnect();
communicator2 = null;
}
}
}
}
