Balls out fun with the Sphero and .NET

Having received a Sphero robotic ball (made by Orbotix) as a christmas present from my wife (yes, she is truly amazing) I went into a frenzy of coding over the holidays. Robotics have always interested me but I never got around to experimenting with it before now.

For those of you looking to tinker a bit with robotics I cannot recommend this little gadget highly enough.  The Sphero is a hermetically sealed spherical shell of polycarbonate crammed full of robotic goodness. Built in sensors include an accelerometer, a compass, and a gyroscope. All communication with the Sphero is performed via Bluetooth through the sealed shell which makes sense as it is intended to be used outdoors and even in water (it floats). The device is thoroughly documented which allows for some very satisfying hacking activity, something which seems to be encouraged by Orbotix, the company behind this little marvel.

sphero

Driving the Sphero around the livingroom using a smartphone for a controller is fun and extremely entertaining for adults, offspring and felines alike, although, for a device promoted as a Robot, it started to feel more and more like a radio controlled ball. A true robot is supposed to scurry around the place and do stuff without me having to frantically wave my phone around. After searching the developer forums for a while I found that there is actually an implementation of BASIC (orbBasic) which will run on the Sphero and allow it to exhibit more autonomy. My first goal became to find a way to upload an orbBasic program to the Sphero from a .NET application and run it.

Sadly the developer SDK’s are only available for mobile platforms (namely Android and iOS) for now. Luckily the low-level documentation for the Sphero is excellent and allowed me to ping the device via Bluetooth from a .NET application and receive a reply within an hour or so of tinkering.

After some additional work and pooring over the documentation I now have a fairly decent (if hurried) experimental framework SpheroNET v0.1 up and running. Please note that it IS experimental and is likely to throw exceptions the moment you look at it funny. The solution includes a console application for testing but nothing else.

What was needed:

  1. A Sphero robotic ball.
  2. A way of sending and receiving data over bluetooth in .NET. For this I used the excellent (and free) 32feet.NET.
  3. The API documentation
  4. orbBasic documentation

First we need to find some devices to connect to.

BluetoothClient client = new BluetoothClient();
List<BluetoothDeviceInfo> devices = new List<BluetoothDeviceInfo>()
devices.AddRange(client.DiscoverDevices());

Once we have retrieved a list of available devices and selected your sphero from among them we need to connect to it. Note that we have to allow for a number of retries. This is because the connection process would otherwise, for unknown reasons, frequently fail. A succesful connection returns a NetworkStream which you will be able to write to and read from.

private NetworkStream Connect(BluetoothDeviceInfo device, int retries)
{
  BluetoothAddress addr = device.DeviceAddress;
  Guid serviceClass = BluetoothService.SerialPort;
  var ep = new BluetoothEndPoint(addr, serviceClass);
  for (int i = 0; i < retries; i++)
  {
    try
    {
      _client.Connect(ep);
      break;
    }
    catch (Exception ex)
    {
      Thread.Sleep(300);
      if (i == (retries - 1))
        throw new Exception(
        string.Format("Could not connect after {0} retries.", retries), ex);
    }
  }
  NetworkStream stream = _client.GetStream();
  return stream;
}

Data sent to the Sphero needs to be formatted into binary sequences i.e. packets that it can understand. There are three types of packets; command, response and asynchronous packets. The format for all three is quite similar which is why I chose to base all packets on a single abstract base class. The primary function of this class is to calculate and update the packet checksum as well as to allow access to the two fields (SOP1 and SOP2) common to all three packet types.

public abstract class SpheroPacket
{
  protected byte[] _data = null;
  public bool IsValid
  {
    get
    {
      return CalculatedChecksum.HasValue ?
       Checksum == CalculatedChecksum.Value :
       false;
    }
  }

  public byte[] Data
  {
    get
    {
      return _data;
    }
  }

  public byte SOP1
  { get { return _data[0]; } }

  public byte SOP2
  { get { return _data[1]; } }

  public byte Checksum
  {
    get { return _data[_data.Length - 1]; }
    set { _data[_data.Length - 1] = value; }
  }

  public byte? CalculatedChecksum
  { get { return GetChecksum(); } }

  public SpheroPacket()
  {
    _data = new Byte[] { 0xFF, 0xFF };
  }

  public void UpdateChecksum()
  {
    byte? checksum = GetChecksum();
    if (checksum.HasValue)
    {
      _data[_data.Length - 1] = checksum.Value;
    }
  }

  public byte? GetChecksum()
  {
    if (_data == null || _data.Length < 4) return null;
    uint sum = 0;
    for (int i = 2; i < _data.Length - 1; i++)
    {
      sum += _data[i];
    }
    return ((Byte)~(sum % 256));
  }

  public override string ToString()
  {
    const string invalid = "[invalid checksum!]->";
    byte[] data = Data;
    StringBuilder sb = new StringBuilder(data.Length * 3);
    if (!IsValid) sb.Append(invalid);
    foreach (var b in data)
    {
      sb.Append(string.Format("{0:X02}", b));
    }
    return sb.ToString();
  }
}

Much of what you might want to do with the Sphero can be accomplished by simply sending properly formatted command packets. The SpheroCommandPacket class below should be able to produce any command described in the API documentation if configured correctly through its constructor.

public class SpheroCommandPacket : SpheroPacket
{
  public byte DeviceId
  { 
    get 
    { 
      return _data[2]; 
    } 
    set
    {
      _data[2] = value; 
      UpdateChecksum(); 
    } 
  }

  public byte CommandId
  { 
    get 
    { 
      return _data[3]; 
    } 
    set
    {
      _data[3] = value;
      UpdateChecksum();
    }
  }

  public byte SequenceNumber
  { 
    get
    {
      return _data[4];
    }
    set
    {
      _data[4] = value;
      UpdateChecksum();
    }
  }

  public byte DataLength
  { 
    get
    {
      return _data[5];
    }
    set
    { 
      _data[5] = value;
      UpdateChecksum();
    }
  }

  public SpheroCommandPacket(
    byte deviceId, byte commandId, 
    byte sequenceNumber, byte[] data): base()
  {
    List<byte> list = new List<byte>();
    list.AddRange(_data);
    list.AddRange(new Byte[] { deviceId, commandId, sequenceNumber });
    if (data != null)
    {
      list.Add((byte)(data.Length + 1));
      list.AddRange(data);
    }
    else
    {
      list.Add(0x01);
    }
    list.Add(0xFF); // Placeholder for checksum
    _data = list.ToArray();
    UpdateChecksum();
  }
}

Obtaining properly formatted command packets is a simple matter of implementing a factory class for that purpose.

public static SpheroCommandPacket EraseOrbBasicStorage(StorageArea area)
{
  return new SpheroCommandPacket(0x02, 0x60, 0x01, new byte[] { (byte)area });
}

public static SpheroCommandPacket AppendOrbBasicFragment(StorageArea area, string fragment)
{
  List<byte> data = new List<byte>();
  byte[] fragBytes = Encoding.Default.GetBytes(fragment);
  data.Add((byte)area);
  data.AddRange(fragBytes);
  return new SpheroCommandPacket(0x02, 0x61, 0x01, data.ToArray());
}

public static SpheroCommandPacket ExecuteOrbBasicProgram(StorageArea area, UInt16 fromLine)
{
  byte[] data = new byte[3];
  data[0] = (byte)area;
  data[1] = (byte)((fromLine & 0xFF00) >> 8);
  data[2] = (byte)(fromLine & 0x00FF);
  return new SpheroCommandPacket(0x02, 0x62, 0x01, data);
}

public static SpheroCommandPacket AbortOrbBasicProgram()
{
  return new SpheroCommandPacket(0x02, 0x63, 0x01, null);
}

Below we send the assembled packet over the NetworkStream. Notice how we also assign a sequence number (one byte) to the packet. This is because the sequence number is echoed in any resulting response packets which will hopefully allow you to figure out which response belongs to which command. How you go about relating asynchronous packages from the Sphero to the commands that triggered them is another question for which I have no definitive answer at the moment. Asynchronous packets may be sent as a result of commands or may be sent as a result of some internal trigger in the Sphero (for example just before it goes asleep).

public void SendPacket(SpheroCommandPacket packet)
{
  packet.SequenceNumber = GetNextSequenceNumber();
  Stream.Write(packet.Data, 0, packetData.Length);
}

Sending an orbBasic program to the Sphero is now quite straightforward. The program below, once sent and executed, will make the Sphero glow a steady green as long as it is not disturbed. Once a preset acceleration threshold is reached it will pulse red for about 10 seconds. This is, admittedly, not a very exciting program but it is a good start.

10 RGB 0, 255, 0
20 if accelone > 1700 then goto 40
30 goto 20
40 for J = 1 to 8
50 for I = 0 to 255 step 10
60 RGB I, 0, 0
70 delay 50
80 next I
90 next J
100 goto 10

For a fuller picture of how you go about receiving and classifying incoming packages (response or async) please refer to the full SpheroNET source code.

For the future I have a number of improvements planned. Firstly I will spend some more time writing orbBasic code to make the Sphero behave like the robot it is, much to the delight, I would guess, of children and cats alike. Secondly all the commands listed in the API documentation should be implemented. An intuitive way of relating sent command packages with their resulting response packets (and asynchronous packets when possible) should also be found.

As always any feedback is welcome.

13 thoughts on “Balls out fun with the Sphero and .NET

  1. Very nice… I started with Sphero a few weeks back to lure my son into programming… Now, I’m on a mission to do a little project/video including Sphero among other things… Is it Ok if use this code in it (I’ll keep the copyrights clear :-)).

    1. It was my pleasure to watch what you made of it! The message in your video is one that I think all passionate programmers can agree with. Programming is so much more than just a profession, it is a way of looking at the world.

  2. Hi, I’m a bit late to the game here, but many thanks for the pointers. Are you still doing any programming with Sphero? I think (I could be wrong) that you’re discarding incomplete packets that are sent as responses? I was finding SpheroNet seemed to be dropping some messages that I was expecting, e.g. an orbbasic loop that included 10 print lines. Having dug around in the code, and had a look at the .NET reference for BeginRead and EndRead, I don’t think the NetworkStream guarantees whole packets will be read. Sometimes you get half a packet, sometimes 1 and a half.

    I altered the code to use a Queue, rather than a byte array. Packets are extracted once they are complete, and any half completed data is left in the queue to see if it gets completed.

    1. It’s more like I left the game early 🙂 My Sphero efforts started as a holiday project (last Christmas) and I sadly didn’t find the time to continue it. I’m very glad that you (and others) have found the effort useful to you.

      You are right that the NetworkStream doesn’t guarantee that entire packets will be read. The code attached to the post should handle this though. Each cycle of BeginRead > EndRead simply reads some bytes which go into the buffer (List). Once an entire packet is in the buffer that package is removed from the buffer and the process continues. This is not to say that the code might not have a bug. I focused more on writing data than on reading asynchronously.

      If I’m granted a peaceful christmas I would love to pick up where I left off. Does anyone have a Sphero 2.0 to spare? 🙂

      1. Hi, that’s no problem. My solution is spread over a few classes I can’t really paste in here, so here is a link to my code:

        https://dl.dropboxusercontent.com/u/37526922/sphero.zip

        The classes you’re interested in are an updated SpheroListener, PacketReader and DataQueue. Thomas’ original SpheroListener reads in a chunk of data, then if it’s a viable packet it processes it. I simply added a queue, and if the data isn’t yet a complete packet, it waits to see if more arrives.

        DataQueue is a simple queue of bytes, SpheroListener copies new messages onto the end of the queue. Then PacketReader reads off any complete packets, and leaves the rest of the data in the Queue for next time.

        Hope it works for you!

    2. Editor works nice Michael, well for short programs, for longer ones I always get response orbBasic error ASCII after I upload it and try run, Same code works fine when I send it via original Thomas client. Maybe issue in fragmets sending to basic memory?

  3. Hi Thomas! I found this through Google and it’s a great start for my own Sphero programming – thanks! 🙂

    Is your code under any particular license? If I wanted to use it in a project that was available publicly on GitHub, would that be ok? (I’d give you credit of course!)

  4. Just got my sphere and successfully used your framework to start develop orbBasic and upload/run in my Sphero 2.0. Thanks a lot for this, it’s lot of fun! By any chance some insight what to change to be comms more reliable as Michael suggested above? What to change in code to use Queue?

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s