WeDo 2.0 Tilt Sensor

This post is part 6 of 6 of  WeDo 2.0 - reverse engineering

 

 

Thanks to Google, I already knew how to set the Tilt Sensor to «mode 1» and then read it. Unfortunately I was not able to understand the data format used by the sensor until I read the recently released SDK.

So the Tilt Sensor has 3 operating modes:

  • Angle (the default mode) = 0
  • Tilt = 1
  • Crash = 2

In this post I’ll explain how to read just the Tilt Mode as is similar to the original WeDo Tilt Sensor operating mode (and also because I still have to organize all my SDK notes).

The WeDo 2.0 ports can also be configured to read analog values in 3 formats: RAW (0), PERCENTAGE (1) and SI (2). Again, I will use just the SI format.

To change the operating mode to Tilt Mode we write this command to the Input Command characteristic (0x003a):

0102012201010000000201

The meaning is similar to the command used in last post for setting the RGB LED to Absolute Mode, the only difference is the Device Type (22h is the Tilt Sensor).

In Tilt Mode and SI Format there are 6 possible values we can read from the Tilt Sensor:

TILT_SENSOR_DIRECTION_NEUTRAL = 0
TILT_SENSOR_DIRECTION_BACKWARD = 3
TILT_SENSOR_DIRECTION_RIGHT = 5
TILT_SENSOR_DIRECTION_LEFT = 7
TILT_SENSOR_DIRECTION_FORWARD = 9
TILT_SENSOR_DIRECTION_UNKNOWN = 10

These values are read from the SensorValue characteristic (0x0032) in a 6-byte format where the first 2 bytes are always “0201” and the other 4 bytes are a IEEE754 float format representation of the value – and now I finally know how to understand those values!

So with just a crude python script we can use the WeDo 2.0 Hub with a Tilt Sensor to control a Mindstorms EV3 car:

#!/usr/bin/env python

# run with sudo

from ev3dev.auto import *
from gattlib import GATTRequester
from time import sleep

BTdevice = "hci0"       # BlueTooth 4.0 BLE capable device

WeDo2HubAddress  = "A0:E6:F8:1E:58:57"

SensorValue_hnd  = 0x32
InputCommand_hnd = 0x3a

# assume TiltSensor at Port #1, change to Mode 1 (Tilt) and Unit = SI
# TiltCmdMode1= "0102012201010000000201"
TiltCmdMode1 = str(bytearray([01,02,01,22,01,01,00,00,00,02,01]))

# Sensor value should read:
# "0201" + a 4 byte value (LITTLE_ENDIAN)
# representing a float in IEEE754 format
# http://www.h-schmidt.net/FloatConverter/IEEE754.html

TILT_SENSOR_DIRECTION_NEUTRAL  = "0000" # 0
TILT_SENSOR_DIRECTION_BACKWARD = "4040" # 3
TILT_SENSOR_DIRECTION_RIGHT    = "a040" # 5
TILT_SENSOR_DIRECTION_LEFT     = "e040" # 7
TILT_SENSOR_DIRECTION_FORWARD  = "1041" # 9
TILT_SENSOR_DIRECTION_UNKNOWN  = "2041" # 10

DELAY      = 0.3
STEP       = 0.175
DT         = 75

tilt_data=[0,0,0,0,0,0]

m1 = LargeMotor(OUTPUT_A)
m2 = LargeMotor(OUTPUT_B)

m1.run_direct()
m2.run_direct()
m1.duty_cycle_sp=0
m2.duty_cycle_sp=0

req = GATTRequester(WeDo2HubAddress,True,BTdevice)
sleep(DELAY)


# try reset - none works
# req.write_by_handle(InputCommand_hnd,"\x22\x44\x11\xAA")
# req.write_by_handle(InputCommand_hnd,"\x44\x11\xAA")
# sleep(DELAY)

# Configure Tilt Sensor
req.write_by_handle(InputCommand_hnd,TiltCmdMode1)
sleep(DELAY)

while True:
  # read Sensor
  data=req.read_by_handle(SensorValue_hnd)[0]

  if data:
    for i,b in enumerate(data):
      tilt_data[i]=ord(b)

    if (tilt_data[4] == 0x00) and (tilt_data[5] == 0x00):
      print("NEUTRAL")
    elif (tilt_data[4] == 0x40) and (tilt_data[5] == 0x40):
      print("BACKWARD")
      m1.duty_cycle_sp=-DT
      m2.duty_cycle_sp=-DT
      sleep(STEP)
      m1.duty_cycle_sp=0
      m2.duty_cycle_sp=0
    elif (tilt_data[4] == 0xa0) and (tilt_data[5] == 0x40):
      print("RIGHT")
      m1.duty_cycle_sp=-DT
      m2.duty_cycle_sp=DT
      sleep(STEP)
      m1.duty_cycle_sp=0
      m2.duty_cycle_sp=0
    elif (tilt_data[4] == 0xe0) and (tilt_data[5] == 0x40):
      print("LEFT")
      m1.duty_cycle_sp=DT
      m2.duty_cycle_sp=-DT
      sleep(STEP)
      m1.duty_cycle_sp=0
      m2.duty_cycle_sp=0
    elif (tilt_data[4] == 0x10) and (tilt_data[5] == 0x41):
      print("FORWARD")
      m1.duty_cycle_sp=DT
      m2.duty_cycle_sp=DT
      sleep(STEP)
      m1.duty_cycle_sp=0
      m2.duty_cycle_sp=0
    elif (tilt_data[4] == 0x20) and (tilt_data[5] == 0x41):
      print("UNKNOWN")

  else:
    print("No data")

  sleep(0.1)

Just a warning note: I ran a first version of this script in my Ubuntu laptop (without the EV3 parts) and it always fails at first run (“No data”, no matter what I do with the Tilt Sensor) but if I abort the script and run it again while the Hub is still in discovering mode then it always works. But when the same script runs in the EV3 it almost always works at first try.

And of course I do have a good explanation: gremlins! What else?

WeDo 2.0 colors with python (again)

This post is part 5 of 6 of  WeDo 2.0 - reverse engineering

After some head aches with the WeDo 2.0 SDK I found out that the WeDo 2.0 Hub has 2 modes for the RGB LED device:

  • Indexed
  • Absolute

“Indexed” is the one I used before – only 10 colors are available. This the mode used in the WeDo 2.0 App, and when in this mode the command used to write to the RGB LED accepts only one byte as argument, which works as an “index” to 10 predefined colors. Why only 10 if the same byte can address up to 255 colors? Internal memory limitations?

The same command accepts  also 3 one-byte arguments (Red, Green and Blue) but only when the RGB LED mode is “Absolute”. This is clear in the SDK… what is not so clear is how to change from default (power on) Indexed mode to Absolute?

After reading many Java files I found out how: we use the “Input Command” characteristic (handle 0x3a) and send it this command:

0102061701010000000201

I’ll explain the format of the “Input Command” in another post, but for now this is the meaning:

  • first two bytes (0102) is the header used to set definitions
  • the third is the port of the device (06 is the RGB LED)
  • the fourth is the type of the device (17h a RGB LED)
  • the fifth is the mode (01 is Absolute, 00 is Indexed)
  • the sixth to nineth bytes is the delta for notifications to be noticed (so 01 00 00 00 = 1d is the minimum)
  • the tenth byte is the unit format to be used (02 is “SI”, internation standard)
  • the eleventh and last byte is to disable or enable notifications (01)

So we can now use any of the 16777216 color tones available:

The pyhton script used for the video above (the video just shows a small part)

#!/usr/bin/python
# run with sudo

from gattlib import GATTRequester
from time import sleep

BTdevice = "hci0"    # BlueTooth 4.0 BLE capable device
WeDo2HubAddress  = "A0:E6:F8:1E:58:57"
InputCommand_hnd = 0x3a
OutputCommand_hnd  = 0x3d
RGBAbsoluteMode_cmd = str(bytearray([01,02,06,17,01,01,00,00,00,02,01]))
RGBAbsoluteOutput_cmd = str(bytearray([06,04,03]))

DELAY      = 0.3

req = GATTRequester(WeDo2HubAddress,True,BTdevice)
sleep(DELAY)

# configure RBG LED to Absolute Mode
req.write_by_handle(InputCommand_hnd,RGBAbsoluteMode_cmd)

# loop all colors
while True:
  for blue in range (0,256,16):
    for green in range (0,256,16):
      for red in range (0,256,16):
        req.write_by_handle(OutputCommand_hnd, RGBAbsoluteOutput_cmd+chr(red)+chr(green)+chr(blue))

 

WeDo 2.0 colors with python

Now that I can finally use LEGO Education WeDo 2.0 app let’s use my latest crazy aquisition: the Ubertooth One (an open source Bluetooth sniffer).

I used a simple project to change the WeDo 2.0 Hub color to ‘pink’ (‘1’) each time I click the ‘Play’ block:

WeDo 2.0 - change color

After capturing a few packets I selected just the interesting part:

1    0.000000000            LE LL    33    Empty PDU
2    0.037564000            ATT    44    UnknownDirection Write Command, Handle: 0x003d
3    0.074999800            LE LL    33    Empty PDU
4    0.187500200            LE LL    33    Empty PDU
5    0.225000800            LE LL    33    Empty PDU
6    0.262503000            LE LL    33    Empty PDU
7    0.262732400            LE LL    33    Empty PDU
8    0.375069400            ATT    44    UnknownDirection Write Command, Handle: 0x003d
9    0.412505800            LE LL    33    Empty PDU

Most frames are empty but 2 frames (‘2’ and ‘8’) show some writing to the 0x003d handle (the same handle that is used to control the motors). Let’s inspect those frames:

Frame 2: 44 bytes on wire (352 bits), 44 bytes captured (352 bits) on interface 0
PPI version 0, 24 bytes
DLT: 147, Payload: btle (Bluetooth Low Energy Link Layer)
Bluetooth Low Energy Link Layer
Bluetooth L2CAP Protocol
Bluetooth Attribute Protocol
    Opcode: Write Command (0x52)
        0... .... = Authentication Signature: False
        .1.. .... = Command: True
        ..01 0010 = Method: Write Request (0x12)
    Handle: 0x003d
    Value: 06040101
Frame 8: 44 bytes on wire (352 bits), 44 bytes captured (352 bits) on interface 0
PPI version 0, 24 bytes
DLT: 147, Payload: btle (Bluetooth Low Energy Link Layer)
Bluetooth Low Energy Link Layer
Bluetooth L2CAP Protocol
Bluetooth Attribute Protocol
    Opcode: Write Command (0x52)
        0... .... = Authentication Signature: False
        .1.. .... = Command: True
        ..01 0010 = Method: Write Request (0x12)
    Handle: 0x003d
    Value: 06040103

So the App is writing “06040101” to the handle and then “06040103”.

I already know something about this handler from the motor control examples I found before:

  • first byte is the “port” identifier (’01’ and ’02’ are the physical plugs to connect motors or sensors so ’06’ is the LED port)
  • second byte is the command sent to the port so ’04’ means “change color”
  • third byte is the length of the arguments of the command so ’01’ means that the color is just one byte length – the fourth

So ’01’ and ’03’ must be some colors right? And I already know that ‘1’ in the App means ‘red’ so probably “06040101” means “change color to pink” and “06040103” means “change color to blue” (blue is the color of the WeDo Hub while waiting for commands).

So let’s test it with gatttool:

char-write-cmd 3d 06040101

Yes! It turns red indeed!

After testing other values I got these 11 values:

00 off
01 pink
02 purple
03 blue
04 cyan
05 light green
06 green
07 yellow
08 orange
09 red
0A light blue

(the real color labels may differ as I’m not very good with colors)

Now a small python script to cycle through all those colors:

#!/usr/bin/python

from gattlib import GATTRequester
from time import sleep

colors = ['\x00','\x01','\x02','\x03','\x04','\x05','\x06','\x07','\x08','\x09','\x0A']

req = GATTRequester("A0:E6:F8:1E:58:57",True,"hci0")

while True:
  for color in colors:
    req.write_by_handle(0x3d, "\x06\x04\x01" + color)
    sleep(2)

And, of course, the video: