WeDo 2.0 Tilt Sensor

 

 

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?

Series Navigation<< WeDo 2.0 colors with python (again)

Deixe uma resposta

O seu endereço de email não será publicado. Campos obrigatórios marcados com *