Controlling Sphero with Windows Phone 8
This article explains how to control a Sphero ball using Windows Phone 8 Bluetooth stack
Windows Phone 8
Introduction
I went to the Bluetooth session at Build 2013 (). The session itself was so-so (only 30min, and not enough details for me) but we got a nice surprise at the end : the attendees (at least the firsts of them) will get a Sphero !
Sphero is a ball than can be controlled by Bluetooth. There are several SDKs, notably for iOS ,Android and a preview version for Windows 8.1.
More info here :
Now let’s play with it !, as we will be using Bluetooth, we need to set the ID_CAP_PROXIMITY, ID_CAP_NETWORKING capabilities in the manifest. Then, just a few lines of code are needed to connect :
PeerFinder.AlternateIdentities["Bluetooth:Paired"] = "";
var pairedDevices = await PeerFinder.FindAllPeersAsync();
// Get the device with "sphero" in the name
var sphero = pairedDevices.FirstOrDefault(d => d.DisplayName.ToUpper().Contains("SPHERO"));
if (sphero == null)
{
MessageBox.Show("No Sphero detected !");
return;
}
try
{
// open a socket
spheroSocket = new StreamSocket();
await spheroSocket.ConnectAsync(spheroName, "1");
}
catch (Exception ex)
{
//...
}
// Get device name
DeviceName = sphero.DisplayName;
Now you’ve got a socket open where you can send the commands
Sending commands
Here is the generic method I use to send the commands
public async Task SendCommand(byte[] command)
{
// Compute checksum
byte checksum = 0;
for (int i = 2; i < command.GetLength(0) - 1; i++)
checksum += command[i];
// set the complement to 1 of the checksum at the end of the message
command[command.GetLength(0) - 1] = (byte)~checksum;
// stream it
await _spheroSocket.OutputStream.WriteAsync(command.AsBuffer());
await _spheroSocket.OutputStream.FlushAsync();
// Show message in output window
Debug.WriteLine(BitConverter.ToString(command));
}
The command byte array is send as parameter. I sum the bytes, from DID to the one before the last (CHK). I invert it, then I put it at the end of the message.
After, I just write it in. Finally I found a message on a forum from a developer of the API who stated that they planned to do it as written, but the new response broke their SDK (a bit annoying!) so they revert to an old response message. Without changing the doc it seems.
Here is what I have on my Sphero (There is a byte on 8th position I have no idea what it is for)
Here is a VersioningInfo class I made :
public class VersioningInfo
{
public string RecordVersion { get; set; }
public string Model { get; set; }
public string HardwareVersion { get; set; }
public string MainAppVersion { get; set; }
public string MainAppRevision { get; set; }
public string BootloaderVersion { get; set; }
public string OrbBasicVersion { get; set; }
public string MacroExecutiveVersion { get; set; }
public string APIVersion { get; set; }
static public VersioningInfo FromBytes(byte[] array)
{
if ((array == null) || (array.GetLength(0) < 9))
throw new ArgumentException();
var stringarray = BitConverter.ToString(array).Split('-');
var obj = new VersioningInfo
{
RecordVersion = stringarray[0],
Model = stringarray[1] == "02" ? "Sphero" : "Unknown",
HardwareVersion = stringarray[2],
MainAppVersion = stringarray[3],
MainAppRevision = stringarray[4],
BootloaderVersion = stringarray[5][0] + "." + stringarray[5][1],
OrbBasicVersion = stringarray[6][0] + "." + stringarray[6][1],
MacroExecutiveVersion = stringarray[7][0] + "." + stringarray[7][1]
};
return obj;
}
}
So, to send the message and get the response :
async public Task<VersioningInfo> GetFirmware()
{
byte[] getFirmwareCommand = { 0xff, 0xff, 0x0, 0x2 ,0x1,0x1,0xFB};
await _spheroSocket.OutputStream.WriteAsync(getFirmwareCommand.AsBuffer());
await _spheroSocket.OutputStream.FlushAsync();
var buffer = new byte[14];
// read first 5 bytes to have first part of message
var b = await _spheroSocket.InputStream.ReadAsync(buffer.AsBuffer(), 5, InputStreamOptions.None);
var response = b.ToArray();
Debug.WriteLine("Answer : " + BitConverter.ToString(response));
var messageresponse = response[2];
// read rest of message
b = await _spheroSocket.InputStream.ReadAsync(buffer.AsBuffer(), response[4], InputStreamOptions.None);
if (messageresponse == 0)
{
// OK, so create a VersionInfo of it
return VersioningInfo.FromBytes(b.ToArray());
}
else
{
throw new Exception("Error code : " + messageresponse);
}
return null;
}
I send the message (not in stream mode, as I need the answer).
I get the first 5 bytes of the answer. I get the message response code (the third byte) and I get the rest of the message .If it is success, it should be 9 bytes. If not, just one (the CHK). Anyway, I get the remaining length using the 5th byte of the answer (DLEN).
Now I check if the response is successful or not. If not, I thrown an exception. If success, I create a VersioningInfo.
Roll
That is probably the main command !
Not complicated. Just set a heading and a speed. Heading is 0 to 359. 0 being straight line on positive Y axis.
async public Task Move(int heading, int speed)
{
if (speed > 255)
speed = 255;
if (speed < 0)
speed = 0;
if (heading < 0)
heading += 360;
if (heading > 359)
heading -= 360;
var command = { 0xff, 0xfe, 0x2, 0x30, 0x1, 0x5, 0, 0, 0, 0x1, 0 };
var directionbytes = BitConverter.GetBytes(heading);
command[6] = (byte)speed;
command[7] = directionbytes[1];
command[8] = directionbytes[0];
SendCommand(command);
}
I check if speed and heading are in correct range.
Then I get a convert the heading into bytes.
I set the heading and speed bytes in the message.
I send the message
{Warning|the STATE byte is set to 1. If you want to make Sphero to stop, it is better to set it to 0, in order to give a smooth deceleration (See appendix C in docs).}}
Sleep
This is a very simple version of the sleep command. No timeout, no macro.
Calibrating
As I said earlier, as Sphero is a ball, there is no real front and back. But we still need to calibrate Sphero to set the heading 0 (straight line).
The way I do it is :
- Set the tail light on so I can have a mark
- Rotate the Sphero by calling the Roll command on a heading with a speed of 0 to align it as you need..
To get sensor data,I use the Motion class
When the orientation changes, I compute the heading and speed :
void _gyroscope_CurrentValueChanged(object sender, SensorReadingEventArgs<MotionReading> e)
{
//Debug.WriteLine(string.Format("X : {0} Y : {1}", e.SensorReading.Attitude.Pitch, e.SensorReading.Attitude.Roll));
var x = e.SensorReading.Attitude.Roll;
var y = e.SensorReading.Attitude.Pitch;
var _halfPi = Math.PI/2;
// Limit to 90 degrees (Pi/2 in radian)
if (x > _halfPi)
x = (float)_halfPi;
if (x < -_halfPi)
x = (float)-_halfPi;
if (y > _halfPi)
y = (float)_halfPi;
if (y < -_halfPi)
y = (float)-_halfPi;
// Move the ball on the screen
Dispatcher.BeginInvoke(() =>
{
((TranslateTransform)Position.RenderTransform).X =
x * (120 / _halfPi);
((TranslateTransform)Position.RenderTransform).Y =
y * (120 / _halfPi);
});
// Get vector angle
var angle = x != 0.0 ? (int)((Math.Atan(y / x) + _halfPi) * (180 / Math.PI)) : 0;
// Get vector magnitude
var speed = Math.Sqrt(x * x + y * y) * 255 / _halfPi;
// If x < 0, add 180 degrees to have a heading in degrees, clockwise
if (x < 0)
angle += 180;
Debug.WriteLine(string.Format("Angle : {0} Speed : {1}", angle, speed));
_spheroCommands.Move(angle, (int)(speed));
}
I limit orientation to 90 degrees max (pi/2). To get the angle, I use the Atan to get an angle (in radian) from a tangent (y/x). I add 90 degrees (pi/2) as for Sphero, heading 0 (angle 0) is on the positive Y axis, not on the positive X as it is by default for math. Then I multiply by 180/Pi so I have degrees and not radians.
For the speed, the norm is simply square root of (x*x + y*y). I divide by Pi/2 (as Pi/2 is the maximum) in order to have a number between 0 and 1. Then, multiply it by 255 to have a number in the range 0..255.
Summary
It is just a quick overview, Sphero is capable of way more, but it needs a book to explain it all. But there is already lot of things that can be done with those simple commands.
You can get the app sources : File:SpheroRT.zip