Sphero API MATLAB SDK

From Mark T. Wiki
Jump to: navigation, search


This Page is a Work in Progress!

Overview and Bluetooth Communication Basics are complete!

I'm actively working on Writing Library Functions

I'm planning to use this page to document the development of the Matlab code from the low-level binary communications API provided by Sphero. Other information about the code such as class documentation, release notes, and change logs might get it's own page on this site (it won't on this one), but most likely will be included only in the m-code itself.

Readers may find it beneficial to read through my explanation of the low-level binary Sphero API before proceeding into the Matlab implementation development contained herein. Official documentation of Sphero API can be found in Sphero API Quick Reference at sphero.com [1] as well as Sphero API Developer Resources documentation on GitHub [2]

Source code can be found on Matlab File Exchange for my current project, Sphero API Matlab SDK, [3] as well as the original Sphero MATLAB Interface [4] authored by Yi Jui Lee.

Check out this video Preview on YouTube to see a glimpse of what this package can do.

Overview

The first step in implementing a programmatic interface to the Sphero API is to establish a Bluetooth connection that will allow you to send and receive data. Thoughout the process of manually scripting device driver code, we build up a repertoire of code snippets that perform redundant functionality. The next stage in development involves writing a set of standard functions that can be used to implement the general features of the API. Since Sphero also has some associated device state information (such as device properties, configuration parameters, and action state), we then proceed to organize this data along with the function library by encapsulation state data and functions into a Sphero device class.

Bluetooth Communication Basics

Assuming that Sphero is paired via Bluetooth on a system running a Matlab installation that includes the instrument control toolbox's Bluetooth object, this section is the place to begin communicating with Sphero. Here, we step through the process of using built-on Matlab commands to: find available Spheros, connect to Sphero via Bluetooth, open a communication port, send data, receive data, close a communication port, and delete the Bluetooth connection to clean up the Matlab workspace. These are the basic elements of Matlab functionality that enable the physical interfacing to Sphero and transmission of data to and from the device.

Find Sphero Devices

The first thing you should do when working with Bluetooth devices in Matlab is use the function instrhwinfo (instrument hardware information) to find information about available Bluetooth devices on your system. Passing a single input argument to specify the type of instrument device, i.e. instrhwinfo('bluetooth'), returns a struct containing all of the information needed to identify the presence of a Sphero device and subsequently instantiate a connection with the device. 

>> hwinfo = instrhwinfo('Bluetooth')
hwinfo = 
        RemoteNames: {2x1 cell}
          RemoteIDs: {2x1 cell}
    BluecoveVersion: 'BlueCove-2.1.1-SNAPSHOT'
     JarFileVersion: 'Version 3.3'

The fields of the output hardware information struct, hwinfo, that are of greatest interest are RemoteNames and RemoteIDs. These are cell arrays of strings containing corresponding names and identifiers for available Bluetooth devices. In this example, we see that there are two Sphero devices paired with the system. 

>> hwinfo.RemoteNames
ans = 
    'Sphero-WPP'
    'Sphero-OOR'

You may notice that name strings in hwinfo.RemoteNames are the same as those displayed in your system's device list. This information is typically sufficient to establish a connection with a unique device if the name is known in advance, say from reading it in your system device list. Possible ambiguities in selecting the appropriate device may be resolved by the knowledge that the last three characters of the name indicate the color sequence that Sphero blinks when woken up. These two physical devices blink the colors white-purple-purple (WPP) and orange-orange-red (OOR). However unlikely, it's still possible for more than one device to have the same name, and in this case, the identifier must be used to indicate the desired device in the next section. 

>> hwinfo.RemoteIDs
ans = 
    'btspp://6886E7061960'
    'btspp://6886E7034E06'

These identifiers are simply six byte hexadecimal hardware addresses (MAC address) that implement the Bluetooth Serial Port Profile (btspp) or RFCOMM. These identifiers should be unique for all Sphero and can be related to the names by their array indices.

In typical application, it's not advised to call the instrhwinfo function unecessarily (i.e. programmatically with every connection attempt) due to the excessive time spent in a single call. Rather, implementations should preferably call this function at most once and store the result. For example, one call can take more than ten seconds. 

>> tic; instrhwinfo('bluetooth'); toc
Elapsed time is 11.683902 seconds.

Create Bluetooth Connection

Once either the name or identifier for Sphero is known, a connection can be made by instantiating a Bluetooth object. There are two minimal signatures for this object constructor. 

>> help bluetooth
 Bluetooth Construct BlueTooth object.
 
    B = Bluetooth( 'RemoteName', Channel ) constructs a Bluetooth
    channel object associated with the remote device ID matching the 
    RemoteName and channel.  RemoteName is a friendly way to identify
    the RemoteID.  If a Channel is not specified, it will default to 0.
 
    B = Bluetooth( 'RemoteID', Channel ) constructs a Bluetooth channel
    object directly from the RemoteID and channel.
...

Either the name or identifier can be used at the first input argument, and for Sphero, the Channel is the number one. Now we're ready to construct a Bluetooth object, bt for the Sphero device named Sphero-WPP. 

>> bt = Bluetooth('Sphero-WPP',1)

   Bluetooth Object : Bluetooth-Sphero-WPP:1

   Communication Settings 
      RemoteName:         Sphero-WPP
      RemoteID:           btspp://6886E7061960
      Channel:            1
      Terminator:         'LF'

   Communication State 
      Status:             closed
      RecordStatus:       off

   Read/Write State  
      TransferStatus:     idle
      BytesAvailable:     0
      ValuesReceived:     0
      ValuesSent:         0

The output of this command reveals some interesting properties of the Bluetooth object.

In the first set of properties, Communication Settings, the only new information is the Terminator property. Since we will not be reading lines of text characters (delimited with a linefeed, for instance), this property has no effect on our intended use of the Bluetooth connection.

The Communication State properties indicate that the communication port is currently closed and that the device is not set up by default to record a text file log of all communications. In the next section we'll open the communication port, and the Matlab help documentation informs proper use of logging features.

The Read/Write State properties, namely the number of bytes available, will be used later when receiving packets to indicate the number of bytes sent from Sphero to Matlab that are ready to be read.

Open Communication Port

After a Bluetooth object, bt, is instantiated, the communication port must be opened in order to read and write data. 

fopen(bt)

The communication port should now be open. If fopen failed, first try waiting a few seconds and issuing the command again. If this doesn't work, then continue to the note below. Verify that the communication port is open by checking the Status property. 

>> bt.Status
ans =
open

You will also notice that Sphero indicates an open Bluetooth communication port by displaying a solid color (typically blue) rather than blinking its three color pattern.

Note that once a Bluetooth object is successfully instantiated and the communication port is opened, the connection must be properly deleted before creating and using a new copy. If this isn't done properly, an error indicating that the device is not available may be thrown. In this event, you may forcibly (and naively) clean up all instrument objects by calling delete(instrfindall) before starting over.

Send and Receive Packets

This Section is a Work in Progress!

Now we're ready to send and receive packets. In this section, we'll follow the command packet encoding and response packet decoding examples for using packets.

Implementation of the Ping command is a good and simple place to begin. We'll walk through the process of using Matlab to build a packet, send the packet data, wait for a response, and finally read the response using this example. Then some examples of implementation of the other commands will follow.

Before sending and receiving packets, it's a good idea to verify that there is no data waiting to be read from the device. We can do this by inspecting the number of incoming BytesAvailable, and, subsequently read that many bytes from the device if this value is greater than zero. 

>> bt.BytesAvailable
ans =
     2

>> if bt.BytesAvailable > 0
  fread(bt,bt.BytesAvailable)
end
ans =
     1
   199

>> bt.BytesAvailable
ans =
     0

Now that the input buffer is known to be clear of meaningless data, we'll move on to writing the CMD packet for the Ping command. A simple way to specify the command bytes in numerical form is to use the function hex2dec. This function takes a cell array of strings (hexadecimal notation) as input and produces the corresponding numerical array. In the following code, we construct the Ping packet as given here, and then write the bytes to the device. 

>> cmd = hex2dec({'FF','FF','00','01','37','01','C6'})'
packet =
   255   255     0     1    55     1   198

>> disp(sprintf('%02X ',cmd)); % print packet in hex
FF FF 00 01 37 01 C6 

>> fwrite(bt,cmd)

Now we expect that Sphero will have received this CMD packet, processed the command, and will soon provide a simple response. We can wait briefly and then inspect the BytesAvailable property to verify that a response has been issued. 

>> bt.BytesAvailable
ans =
     6

Exactly six bytes is what we should expect for the simple response to a Ping command! Now we can read the data into the Matlab workspace for further investigation. 

>> rsp = fread(bt,6)'
rsp =
   255   255     0    55     1   199

>> disp(sprintf('%02X ',rsp)); % print packet in hex
FF FF 00 37 01 C7

This response packet matches perfectly the successful response shown here.

In the remainder of this section, we'll show examples of the code snippets used to perform these operations with other commands.

SetRGBLEDOutput
Makes Sphero red
>> cmd = hex2dec({'FF','FF','02','20','37','05','FF','00','00','00','A2'})' % red
cmd =
   255   255     2    32    55     5   255     0     0     0   162

>> disp(sprintf('%02X ',cmd)); % print packet in hex
FF FF 02 20 37 05 FF 00 00 00 A2 

>> fwrite(bt,cmd)

>> bt.BytesAvailable
ans =
     6

>> rsp = fread(bt,6)'
rsp =
   255   255     0    55     1   199

>> disp(sprintf('%02X ',rsp)); % print packet in hex
FF FF 00 37 01 C7
Roll
Implement the stop command before experimenting with Roll commands that make Sphero move
>> cmd = hex2dec({'FF','FF','02','30','37','05','00','00','00','00','91'})' % stop
cmd =
   255   255     2    48    55     5     0     0     0     0   145

>> disp(sprintf('%02X ',cmd)); % print packet in hex
FF FF 02 30 37 05 00 00 00 00 91 

>> fwrite(bt,cmd)

>> bt.BytesAvailable
ans =
     6

>> rsp = fread(bt,6)'
rsp =
   255   255     0    55     1   199

>> disp(sprintf('%02X ',rsp)); % print packet in hex
FF FF 00 37 01 C7
ReadLocator
>> cmd = hex2dec({'FF','FF','02','15','37','01','B0'})'
cmd =
   255   255     2    21    55     1   176

>> disp(sprintf('%02X ',cmd)); % print packet in hex
FF FF 02 15 37 01 B0 

>> fwrite(bt,cmd)

>> bt.BytesAvailable
ans =
    16

>> rsp = fread(bt,16)'
rsp =
  Columns 1 through 14
   255   255     0    55    11     0     9   255   197   255   143   255   103     0
  Columns 15 through 16
   190    62

>> disp(sprintf('%02X ',rsp)); % print packet in hex
FF FF 00 37 0B 00 09 FF C5 FF 8F FF 67 00 BE 3E 

>> % interpret data
x_pos = typecast( uint8( fliplr(rsp(6:7)   )), 'int16')
y_pos = typecast( uint8( fliplr(rsp(8:9)   )), 'int16')
x_vel = typecast( uint8( fliplr(rsp(10:11) )), 'int16')
y_vel = typecast( uint8( fliplr(rsp(12:13) )), 'int16')
sog   = typecast( uint8( fliplr(rsp(14:15) )),'uint16')
x_pos =
      9
y_pos =
    -59
x_vel =
   -113
y_vel =
   -153
sog =
    190

Close Communication Port

When finished with Sphero communication, you should first close the communication port before properly destroying the Bluetooth object. 

fclose(bt)

Delete Bluetooth Connection

Management of the communication session is very important for future connection attempts. After closing the communication port the Blueooth object must be properly destroyed to release system resources by using delete and clear. 

delete(bt)
clear bt

If these commands are not called in sequence before successively creating a new connection and opening its communication port, you will experience an error. If this happens, you may choose to forcibly (and naively) clean up all existing instrument objects by calling delete(instrfindall) as a workaround.

Simple Example Script

Here's an example script that demonstrates a programmatic implementation of the core concepts involved with using Sphero in Matlab m-code.

Example Script
 1 % create bluetooth object
 2 disp('Creating Bluetooth object ...');
 3 bt = Bluetooth('Sphero-WPP',1);
 4 
 5 % open communication port at most NUM_TRIES times
 6 num = 0; NUM_TRIES = 10;
 7 while strcmp('closed',bt.Status) && num < NUM_TRIES
 8   disp('Attempting to open communication port ...');
 9   pause(1); num = num + 1;
10   try
11     fopen(bt);
12   catch e
13     disp(['Open port failed on attempt number ' num2str(num)]);
14   end
15 end
16 disp('Established open communication port');
17 
18 % send a ping command
19 disp('Sending command ...');
20 cmd = hex2dec({'FF','FF','00','01','37','01','C6'});
21 fwrite(bt,cmd);
22 
23 % read RSP packet
24 disp('Reading RSP packet ...');
25 rsp = []; valid_rsp = false; TIMEOUT = 2;
26 tic;
27 while ~valid_rsp && toc < TIMEOUT
28   % read available data
29   if bt.BytesAvailable > 0
30     rsp = [rsp,fread(bt,1)];
31   end
32   % run state machine on incoming data to read a RSP packet
33   % RSP: [SOP1,SOP2,MRSP,SEQ,DLEN,<DATA>,CHK]
34   if length(rsp) > 0 && rsp(1) ~= hex2dec('FF')
35     % invalid SOP1
36     rsp = rsp(2:end); % shift out first byte
37   elseif length(rsp) > 1 && rsp(2) ~= hex2dec('FF')
38     % invalid SOP2 (coincidentally, also invalid SOP1)
39     rsp = rsp(3:end); % shift out first two bytes
40   elseif length(rsp) < 5
41     % waiting for dlen byte
42   elseif length(rsp) < rsp(5) + 5
43     % waiting for full packet
44   else
45     % received full packet
46     valid_rsp = true;
47   end
48 end
49 
50 if ~valid_rsp
51   error('Response packet not received');
52 end
53 
54 % interpret response
55 disp('Interpreting response ...');
56 if rsp(end) ~= bitcmp(mod(sum(rsp(3:end-1)),256),'uint8');
57   disp('FAILURE: CHK indicates corrupt or misaligned packet');
58 elseif rsp(3) ~= 0
59   disp('FAILURE: MRSP indicates error in processing CMD');
60 else
61   disp('SUCCESS!');
62 end
63 
64 fclose(bt);
65 delete(bt);
66 clear bt;
Example Script Output
Creating Bluetooth object ...
Attempting to open communication port ...
Established open communication port
Sending command ...
Reading RSP packet ...
Interpreting response ...
SUCCESS!

Writing Library Functions

The code examples presented in Bluetooth Communcation Basics offer most of the critical functionality desired in a programmatic interface to Sphero. However, the scripting approach to program development is to write, maintain, and reuse when incorporating Sphero into larger programs. The next step of development involves writing self-contained functions that perform the core functionality of a Sphero interface based upon the previous examples.

In writing this function library, we will adopt a data-oriented model for the function input parameters and output parameters. Since the Sphero device has data that requires bookkeeping, we'll pass a struct variable through function calls to keep track of Sphero's state.

We'll begin with developing functions based upon the logic contained in Yi Jui's Sphero MATLAB Interface. [4] As such, we will only consider sending CMD packets and receiving synchronous RSP packets without the added complexity of continuously monitoring incoming data to receive and handles asynchronous MSG packets. This generalization of control flow and support for Sphero's features will be addressed later in the next section.

Data Model and Initialization

Early on, we've chosen to consciously keep track of Sphero's device state and data in a struct that we'll call s, for Sphero. Although we're only writing a function library at this point, adopting the convention of passing s as the first input argument and returning it as the first output better sets us up for transitioning to a device class later on.

Before really working on the core functionality of the function library, we'll begin by writing a function that constructs s. Here, we'll choose to name the function based upon the object that the data represents, i.e. Sphero.

Sphero function source
function s = Sphero()
s.bt = []; % initialize empty field for Bluetooth object
end
Sphero function usage
s = Sphero();

Connection Utility

Once Sphero is used to create an empty Sphero object, we need a way to create and open a Bluetooth connection to Sphero.

connectBluetooth function source
function s = connectBluetooth(s,remote_name)
if isempty(s.bt) % initialize 
  s.bt = Bluetooth(remote_name, 1);
else % assume bt is a Bluetooth object and force it closed
  fclose(s.bt);
end
fopen(s.bt);
end
connectBluetooth function usage
s = connectBluetooth(s,'Sphero-WPP');

Once a connection is established, we should immediately implement the complementary function that cleans up an open connection.

deleteBluetoothObject function source
function deleteBluetoothObject(s)
if strcmp('open',s.bt.Status) % close an open Bluetooth connection
  fclose(s.bt);
end
delete(s.bt); % delete bluetooth object
end
deleteBluetoothObject function usage
deleteBluetoothObject(s);

At this point, we should be able to open a connection with Sphero and clean up the resources using only library functions and one workspace variable, s. The following example script should function without errors when run repeatedly as long as Sphero and Matlab play nicely together.

Example

clear all; % start with a clean workspace

s = Sphero();
s = connectBluetooth(s,'Sphero-WPP'); % connect to sphero

% ...
% program behavior goes here
% ...

deleteBluetoothObject(s); % clean up when done


Send and Receive

This Section is a Work in Progress!

Implementing API Functions

This Section is a Work in Progress!

Device Class Encapsulation

This Section is a Work in Progress!

Frequently Asked Questions

Can you please guide me through the procedure of parsing the received response from the Set Data Streaming command?
Answer coming soon ...

References

  1. ↑ Sphero Docs API Quick Reference; Source: http://sdk.sphero.com/api-reference/api-quick-reference/
  2. ↑ Sphero Developer Resources (Orbotix) on GitHub; Source: https://github.com/orbotix/DeveloperResources/tree/master/docs
  3. ↑ Sphero API Matlab SDK; Author: Mark Tomaszewski; Source: http://www.mathworks.com/matlabcentral/fileexchange/52746-sphero-api-matlab-sdk
  4. ↑ 4.0 4.1 Sphero MATLAB Interface; Author: Yi Jui Lee; Contributor: Mark Tomaszewski; Source: http://www.mathworks.com/matlabcentral/fileexchange/48359-sphero-matlab-interface
Retrieved from "http://wiki.mark-toma.com/w/index.php?title=Sphero_API_MATLAB_SDK&oldid=247"