Reverse Engineering Sphero R2D2 - I like to move it!
Tags: javascript, tutorial, beginners, node
Published: 2020-04-04 15:24:34

In the [first part of Reverse Engineering Sphero R2D2](https://dev.to/astagi/reverse-engineering-sphero-r2d2-with-javascript-16ip) I made a deep look inside Sphero documentation and used Wireshark to catch all the BLE messages between the phone and the droid, replicating them using `Node.js`. At the end of the first part we were able to animate the droid and rotate the top, now it's time to make our droid move in any direction and play with the accelerometer!

The final result is in this video 📺 Check the final code in [this repository](https://github.com/astagi/freer2)

[![Watch the video](https://img.youtube.com/vi/IjlItzbqFtk/0.jpg)](https://youtu.be/IjlItzbqFtk)

<figcaption class="imageCaption">Moving Sphero R2D2 droid</figcaption>

## R2D2 Movement

Using the [Official Sphero App](https://play.google.com/store/apps/details?id=com.sphero.starwars&hl=en) in "driving mode" you can find a big circle on the left with a small lighting blue point in its center.

![R2D2 Sphero App](https://is3-ssl.mzstatic.com/image/thumb/Purple124/v4/75/f1/47/75f1475e-3a1d-d7b4-fe4a-61b0a0d557bc/pr_source.png/750x750bb.jpeg)

Moving the blue point inside the big circle allows you to move R2D2 around, at a certain speed. R2D2 is also able to move forward and backward. During BLE packets analysis I expect to find packets with these information:

- The heading (from 0° to 360°)
- Direction (forward or backward)
- Speed

That's my scanning result after driving my droid around the room

```
...| 0x0A | 0x16 | 0x07 | 0xB0 | 0x00 | 0xB4 | 0x00 |...
...| 0x0A | 0x16 | 0x07 | 0xC2 | 0x00 | 0xB4 | 0x00 |...
...| 0x0A | 0x16 | 0x07 | 0xFF | 0x00 | 0xB4 | 0x00 |...

...

...| 0x0A | 0x16 | 0x07 | 0x32 | 0x01 | 0x0E | 0x01 |...
...| 0x0A | 0x16 | 0x07 | 0x6A | 0x01 | 0x0E | 0x01 |...
...| 0x0A | 0x16 | 0x07 | 0xA1 | 0x01 | 0x0E | 0x01 |...
```

As you can see, the common part of these messages is `0x0A, 0x16, 0x07` so we can define the const value

```js
const MSG_MOVE = [0x0A, 0x16, 0x07]
```

The next byte contains a value between `0x00` and `0xFF`, it must be the `speed`.

The following 2 bytes look to be the `heading`. I expect to find a value in degrees, so I try to convert these bytes using the [IEEE-754 Floating Point Converter](https://www.h-schmidt.net/FloatConverter/IEEE754.html) as we did in the previous article to move the top

    0x00B4 => 2.52233723578e-43

As you can see, this is not a valid value for the heading. Let's try to convert it to a decimal value

    0x00B4 => 180

Yay, 180 degrees! ✌🏻

As we can easily imagine, the last byte is the `direction` (`0x00` => forward, `0x01` => backward).

Now before start trying to move our droid programmatically, we need a function to convert a degree value to hex. We can modify the existing `convertDegreeToHex` adding integer support. 

```js
const CONVERSIONS = {
  INTEGER: 'i',
  FLOAT: 'f',
};


let convertDegreeToHex = (degree, format = CONVERSIONS.INTEGER) => {
  var view = new DataView(new ArrayBuffer(4));
  format === CONVERSIONS.FLOAT ? view.setFloat32(0, degree) : view.setUint16(0, degree)
  return Array
    .apply(null, {
      length: format === CONVERSIONS.FLOAT ? 4 : 2
    })
    .map((_, i) => view.getUint8(i))
}
```
Give it a try!

```js
convertDegreeToHex(0)
// => [0x00, 0x00]
convertDegreeToHex(180)
// => [0x00, 0xB4]
convertDegreeToHex(270)
// => [0x01, 0x0E]
convertDegreeToHex(270, CONVERSIONS.FLOAT)
// => [0x43, 0x87, 0x00, 0x00]
```

Using the `writePacket` function we can now move our droid with our code 🎉 Let's try to draw a square!

```js
for (let i = 0 ; i < 4 ; i++) {
  await writePacket(
    characteristic,
    buildPacket(
      MSG_MOVE, 
      [0xFF, ...convertDegreeToHex(i * 90), 0x00]
    )
  );
  await new Promise(resolve => setTimeout(resolve, 2000));
}
```

Remember to set a timeout after sending a MSG_MOVE, these message are executed instantly! Also keep in mind that heading takes some time to execute (~450ms for 180° rotation).

## Accelerometer inspection

Accelerometer inspection is the hardest part I found during reverse engineering. Using the official app to move the droid I didn't find anything related to the accelerometer (e.g. collision detection), so I tried to use another app [Sphero Edu] where events like collision detection are supported (https://play.google.com/store/apps/details?id=com.sphero.sprk&hl=en). Using this app we can create simple block scripts to play with our droid! 

Let's make a simple script with collision detection enabled and log BLE communication during its execution

<img src="https://github.com/astagi/freer2/blob/master/assets/spheroedu.jpg?raw=true" alt="Sphero Edu App" width="200"/>

Inspecting Wireshark log you can see that there's a special message sent by Sphero Edu App to our droid

```
| 0x0A | 0x18 | 0x00 | 0x00 | 0x96 | 0x00 | 0x00 | 0x07 | 0xe0 | 0x78 |
```

This message activates an infinite stream of messages like these

```
| 0x8D | 0x00 | 0x18 | 0x02 | 0xFF | 0x41 | 0xE8 | 0xBA | 0x70 | 0x41 | 0x35 | 0xB6 | 0x97 | 0xC1 | 0xAB | 0x50 | 0xDB | ... | 0xD8 |

| 0x8D | 0x00 | 0x18 | 0x02 | 0xFF | 0x42 | 0xE2 | 0xAA | 0x60 | 0x41 | 0x35 | 0xB2 | 0x67 | 0xC1 | 0xBB | 0x20 | 0xAB | ... | 0xD8 |
```

The common part of these messages is 

```
| 0x8D | 0x00 | 0x18 | 0x02 | 0xFF |
```

I expect to find there `X, Y and Z` values. At a first glance, the 12 bytes following the common part, look to be 3 IEEE754 numbers

```
Common part: | 0x8D | 0x00 | 0x18 | 0x02 | 0xFF |
X axis:      | 0x41 | 0xE8 | 0xBA | 0x70 |
Y axis:      | 0x41 | 0x35 | 0xB6 | 0x97 |
Z axis:      | 0xC1 | 0xAB | 0x50 | 0xDB |
```

![XYZ](https://github.com/astagi/freer2/blob/master/assets/xyz.png?raw=true)

We need to modify our code before receiving these data because they may interfere with other data read operations. To avoid this problem use a function to check the "header" of the received packet (`isActionResponse`)

```js
let isActionResponse = (data) => {
  let valid = false;
  valid |= data.slice(0, 2).every((v) => [0x8D, 0x09].indexOf(v) >= 0);
  valid |= data.slice(0, 2).every((v) => [0x8D, 0x08].indexOf(v) >= 0);
  valid |= data.slice(0, 3).every((v) => [0x8D, 0x00, 0x17].indexOf(v) >= 0);
  return valid;
}
```

And add this code before data validation on `writePacket`

```js
let listenerForRead = (data) => {
  
  // ...

  if (eopPosition !== -1) {
    // Check if Package is for me
    if (isActionResponse(dataToCheck)) {
      // Process data
    }
  }
};
```

It's time to create the main function to activate the accelerometer inspection, `enableAccelerometerInspection`. This function have to

- Receive a `characteristic` and a `callback function`
- Write the packet to activate accelerometer inspection
- Read data and decode them (remember the schema?)
![Encode/decode](https://res.cloudinary.com/practicaldev/image/fetch/s--w68y2FUh--/c_limit%2Cf_auto%2Cfl_progressive%2Cq_auto%2Cw_880/https://cdn-images-1.medium.com/max/800/1%2AO6mqYoyE6F4Ki4OY8NJp2w.png)
- Convert X, Y and Z values and send them to the callback

```js
const MSG_ACCELEROMETER = [0x0A, 0x18, 0x00];


let enableAccelerometerInspection = (characteristic, callback) => {
  let dataRead = [];
  let dataToCheck = [];
  let eopPosition = -1;
  characteristic.write(Buffer.from(buildPacket(MSG_ACCELEROMETER, [0x00, 0x96, 0x00, 0x00, 0x07, 0xe0, 0x78])));
  characteristic.on('data', (data) => {
    dataRead.push(...data);
    eopPosition = dataRead.indexOf(EOP);
    dataToCheck = dataRead.slice(0);
    if (eopPosition !== dataRead.length - 1) {
      dataRead = dataRead.slice(eopPosition + 1);
    } else {
      dataRead = [];
    }
    if (eopPosition !== -1) {
      if (dataToCheck.slice(0, 5).every((v) => [0x8D, 0x00, 0x18, 0x02, 0xFF].indexOf(v) >= 0)) {
        // Decode packet
        let packetDecoded = [];
        for (let i = 0; i < dataToCheck.length - 1; i++) {
          if (dataToCheck[i] == ESC && dataToCheck[i + 1] == ESC_ESC) {
            packetDecoded.push(ESC);
            i++;
          } else if (dataToCheck[i] == ESC && dataToCheck[i + 1] == ESC_SOP) {
            packetDecoded.push(SOP);
            i++;
          } else if (dataToCheck[i] == ESC && dataToCheck[i + 1] == ESC_EOP) {
            packetDecoded.push(EOP);
            i++;
          } else {
            packetDecoded.push(dataToCheck[i])
          }
        }
        let x = Buffer.from(packetDecoded.slice(5, 9)).readFloatBE(0);
        let y = Buffer.from(packetDecoded.slice(9, 13)).readFloatBE(0);
        let z = Buffer.from(packetDecoded.slice(13, 17)).readFloatBE(0);
        callback(x, y, z);
      }
    }
  });
}
```

```js
enableAccelerometerInspection(characteristic, (x, y, z) => {
  console.log('----------------------')
  console.log("X:" + x)
  console.log("Y:" + y)
  console.log("Z:" + z)
});
```
Watch this video to see accelerometer in action 📺

[![Watch the video](https://img.youtube.com/vi/3ch7mwRc6hM/0.jpg)](https://youtu.be/3ch7mwRc6hM)

<figcaption class="imageCaption">Log accelerometer</figcaption>

Every second the callback gets called ~ 7 times. With these values you can program incline detection, check if your droid fall on the ground, write a simple collision detection and so on!

## DYALF

It's time to wrap all that we learned during this reverse engineering process in a library to take advantage of OOP and write a better and more reusable code. For this purpose I created the library DYALF (Droids You Are Looking For) containing all the methods to play with R2D2. You can [check the code on Github](https://github.com/astagi/dyalf). With DYALF you can write code like this

```js
const dyalf = require('./dyalf');


let main = async () => {

  let r2 = new dyalf.R2D2('4bef2b0786334e2fac126c55f7f2d057');

  await r2.connect();
  await r2.openCarriage();
  await r2.sleep(1000);
  await r2.animate(7);

  for (var i = -160; i < 180; i += 5) {
    await r2.rotateTop(i);
  }

  await r2.off();

  dyalf.shutdown();

};

main();
```

And is made to support other droids extending the base class `Droid` (BB8 droid support will be ready soon!). 

Using the movement is really simple and readable, rewriting the square drawing function with DYALF will look like

```js
console.log('Make a square 🔳');
for (let i = 0; i < 4; i++) {
  await r2.move(0xFF, i * 90, 3000);
}

await r2.stop();
```

DYALF adds the `time` parameter to move your droid in a specific direction for N milliseconds. 

To get accelerometer values we can simply listen to an event! The base class `Droid` extends `EventEmitter` to support events

```js
const EventEmitter = require('events');


class Droid extends EventEmitter {
```

so you can receive accelerometer values listening to `accelerometer` event!

```js
r2.on('accelerometer', (x, y, z) => {

});
```

If you want to see other funny methods of DYALF, check the `examples` folder containing some useful scripts.

Cover image: artwork by [Susan Murtaugh](https://www.flickr.com/photos/suzi54241/)