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:

EV3DEV na BRInCKa 2015

O controlo dos meus módulos MFL é feito por intermédio de um computador LEGO Mindstorms EV3 a correr ev3dev – Debian Linux for the EV3, uma versão de Debian Linux criada de propósito para o EV3.

EV3DEV inside

O tile à frente do pinguim foi-me oferecido pelos responsáveis do projecto em reconhecimento pelo Rasperry Pi 2 que contribuí para o projecto, que está agora numa fase de ser portado para outros dispositivos similares como o Pi ou o Cubieboard.

Módulo MFL em Tomar

Os meus primeiros 3 módulos MFL (Módulo Ferrovia LEGO) junto dos restantes módulos da PLUG no Tomar BRInCKa 2015:

O  módulo ao centro tem dois desvios motorizados, bem como um «desengatador» no troço de recta entre as duas agulhas. Toda a electrónica está disfarçada por baixo das linhas.

O módulo da direita consiste numa plataforma rotativa motorizada e controlada por um LEGO Mindstorms EV3 correndo ev3dev (uma variante de Debian para o EV3). Sobre o carril um pequeno comboio controlado por SBrick (bluetooth).

O módulo da esquerda, quase 100% da minha marida, começou com uma pequena cascata no nosso presépio do Natal anterior.

A montagem no BRInCKa 2015 não ficou completa por isso todas as demonstrações são manuais, controladas por mim. Neste video gravado em casa é o computador (LEGO Mindstorms EV3) a fazer tudo sozinho:

O que faltou montar e que permite a automação plena? As componentes RFID – por baixo do pequeno comboio há um identificador RFID do tamanho de uma moeda que permite identificá-lo e em certa medida seguir-lhe a posição. Na montagem do video existem dois leitores RFID (USB) por baixo da linha (um no extremo direito e outro junto ao sistema de desengate). O EV3 utiliza esses sensores para perceber se o comboio está nos sítios certos antes de passar à acção seguinte.

Raspberry Pi: novo modelo

Depois das melhorias trazidas com o modelo B+ surge agora um nova revisão do modelo A, o Raspberry Pi A+:

Raspberry Pi A+
  • menos 21 mm de comprimento
  • menor consumo (55% do B+)
  • microSD
  • mais portas GPIO
  • mehor audio
  • menos $5

Para aplicações vocacionadas para o «embedded» que se contentem com 256 MB de RAM, não precisem de rede este modelo torna-se muito apelativo.

R4T1NH0 – a LEGO micro rover

At home with just the kids, used the SBrick for a RC micro rover:

It’s very fragile but I’m certain it is possible to make it even smaller and sturdy, just a matter of time and skill.



[actualized on the next day]

A slightly better version witj less friction at the connection between the weels and the micro-motors. Also tried 3 CR2430 lithium batteries to supply 9 Volt. The batteries behave better than expected.


Almost out of nowhere I now have a table robot with great potential for Snap!

[new actualization]

Robot consumption with 3 fresh CR2430 batteries goes from 60 mA (moving forward) to 90 mA (rotating over itself).

R4T1NH0 – um micro rover LEGO

Sozinho em casa com os miúdos, usei o SBrick para um micro rover telecomandado:

É uma montagem frágil mas estou certo que é possível fazer ainda mais pequeno e ainda assim mais robusto, é uma questão de tempo e engenho.



[actualizado no dia seguinte]

Uma versão ligeiramente melhor em que a ligação das rodas aos micro-motores não gera tanto atrito e onde experimentei utilizar 3 pilhas de lítio CR2430 para fornecer os 9 Volt. As pilhas portaram-se melhor do que eu esperava.


Assim quase do nada tenho aqui um robot de mesa bastante interessante para experiências com o Snap!

[nova actualização]

O consumo do robot com 3 pilhas CR2430 novas varia entre 60 mA (movendo-se para a frente) e 90 mA (rodando sobre si próprio).

Controlling the SBrick with a wiimote

I found out that is really easy to use a wiimote with Linux for so as a sequel to my previous ‘Sbrick – remote control with a wireless gamepad‘ I now present you my ‘Controlling the SBrick with a wiimote’ (actually a cheap clone, a N-Play Remote Plus, unfortunately I haven’t found a version with Motion Plus).

The wiimote uses Bluetooth, but doesn’t strictly follow the rules. If we have bluetooth on our PC and want to check if it will work, just press both ‘1’ and ‘2’buttons for it to advertise for a while and then:

$ hcitool -i hci0 scan
Scanning ...
    04:02:16:01:1C:E7    Nintendo RVL-CNT-01

It’s not possible to pair with the wiimote but there are tools for that, like cwiid. As I’m going to use python I installed their python library:

$ sudo apt-get install python-cwiid

There are lots of examples so I’ll show only my final result:

 

And the script I used for the video above:

# apt-get install python-cwiid
import cwiid
from time import sleep
from subprocess import call
from math import log10

# macros for the SBrick
DRIVE_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0102"
DRIVE_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0103"
COAST_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01020000"
COAST_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01030000"
BREAK_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0002"
BREAK_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0003"


# connecting to the wiimote. This allows several attempts
# as first few often fail.

print 'Press 1+2 on your Wiimote now...'
wm = None
i=1
while not wm:
    try:
        wm=cwiid.Wiimote()
    except RuntimeError:
        if (i>5):
            print("cannot create connection")
            quit()
        print "Error opening wiimote connection"
        print "attempt " + str(i)
        i +=1

#set wiimote to report button presses and accelerometer state
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC

#turn on all led and ruble to show connected
wm.led=15
wm.rumble=True
sleep(0.5)
wm.rumble=False
wm.led=0
sleep(1.5)

# roll = accelerometer[0], standby ~125
# pitch = accelerometer[1], standby ~125

while True:
    buttons = wm.state['buttons']

    #only pay attention when button '1' pressed
    if (buttons & cwiid.BTN_1):

        roll=(wm.state['acc'][0]-125)
        pitch=(wm.state['acc'][1]-125)
        
        if (roll<0):
            if (roll<-4):
                if (roll<-25):
                    roll=-100
                else:
                    roll=-50*log10(-4*roll)
            else:
                roll=0

        if (roll>0):
            if (roll>4):
                if (roll>25):
                    roll=100
                else:
                    roll=50*log10(4*roll)
            else:
                roll=0

        if (pitch>0):
            if (pitch>4):
                if (pitch>25):
                    pitch=100
                else:
                    pitch=50*log10(4*pitch)

            else:
                pitch=0

        if (pitch<0):
            if (pitch<-4):
                if(pitch<-25):
                    pitch=-100
                else:
                    pitch=-50*log10(-4*pitch)
            else:
                pitch=0

        if ((pitch<>0)or(roll<>0)):

            roll=2.5*roll
            pitch=2.5*pitch

            if(pitch<>0):
                if(roll>0):
                    # turn right
                    motor_L=pitch
                    motor_R=-pitch-roll/2

                else:
                    # turn left
                    motor_R=-pitch
                    motor_L=pitch+roll/2

            elif(roll<>0):
                #just rotate
                motor_R=motor_L=roll;

            else:
                # does nothing
                motor_R=motor_L=0

            if((motor_R<>0)or(motor_L<>0)):

                if(motor_R<0):
                    duty=str(hex(int(-motor_R)))
                    command_A=DRIVE_A+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_R)))
                    command_A=DRIVE_A+"01"+duty[2:]

                if(motor_L<0):
                    duty=str(hex(int(-motor_L)))
                    command_B=DRIVE_B+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_L)))
                    command_B=DRIVE_B+"01"+duty[2:]

                #send motors commands to SBrick
                call(command_A, shell=True);
                call(command_B, shell=True);
                sleep(0.1)

                #send COAST commands to SBrick
                call(COAST_A, shell=True);
                call(COAST_B, shell=True);

    else:
        # inactive
        sleep(0.01)

Controlo de SBrick com wiimote

Descobri que é muito fácil usar um wiimote em Linux por isso na sequência de ‘SBrick – controlo remoto com um gamepad‘ segue agora como controlar o SBrick com um wiimote (na verdade um clone barato, um N-Play Remote Plus, comprado fora de horas numa Worten – infelizmente não havia a versão com Motion Plus).

O wiimote usa Bluetooth, apesar de não seguir estritamente as normas. Se tivermos bluetooth no nosso PC podemos confirmar que estamos em condições de utilizar o wiimote carregando em simultâneo nos botões ‘1’ e ‘2’ do wiimote para que este fique visível durante uns segundos:

$ hcitool -i hci0 scan
Scanning ...
    04:02:16:01:1C:E7    Nintendo RVL-CNT-01

Como não segue as normas não é possível emparelhar com ele mas existem ferramentas para isso como o cwiid. Como vou usar pyhton fui buscar a library correspondente:

$ sudo apt-get install python-cwiid

existem vários exemplos básicos que não vou apresentar aqui, sigo directamente para o resultado final:

 

O script utilizado no video acima:

# apt-get install python-cwiid
import cwiid
from time import sleep
from subprocess import call
from math import log10

# macros for the SBrick
DRIVE_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0102"
DRIVE_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0103"
COAST_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01020000"
COAST_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01030000"
BREAK_A="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0002"
BREAK_B="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=0003"


# connecting to the wiimote. This allows several attempts
# as first few often fail.

print 'Press 1+2 on your Wiimote now...'
wm = None
i=1
while not wm:
    try:
        wm=cwiid.Wiimote()
    except RuntimeError:
        if (i>5):
            print("cannot create connection")
            quit()
        print "Error opening wiimote connection"
        print "attempt " + str(i)
        i +=1

#set wiimote to report button presses and accelerometer state
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC

#turn on all led and ruble to show connected
wm.led=15
wm.rumble=True
sleep(0.5)
wm.rumble=False
wm.led=0
sleep(1.5)

# roll = accelerometer[0], standby ~125
# pitch = accelerometer[1], standby ~125

while True:
    buttons = wm.state['buttons']

    #only pay attention when button '1' pressed
    if (buttons & cwiid.BTN_1):

        roll=(wm.state['acc'][0]-125)
        pitch=(wm.state['acc'][1]-125)
        
        if (roll<0):
            if (roll<-4):
                if (roll<-25):
                    roll=-100
                else:
                    roll=-50*log10(-4*roll)
            else:
                roll=0

        if (roll>0):
            if (roll>4):
                if (roll>25):
                    roll=100
                else:
                    roll=50*log10(4*roll)
            else:
                roll=0

        if (pitch>0):
            if (pitch>4):
                if (pitch>25):
                    pitch=100
                else:
                    pitch=50*log10(4*pitch)

            else:
                pitch=0

        if (pitch<0):
            if (pitch<-4):
                if(pitch<-25):
                    pitch=-100
                else:
                    pitch=-50*log10(-4*pitch)
            else:
                pitch=0

        if ((pitch<>0)or(roll<>0)):

            roll=2.5*roll
            pitch=2.5*pitch

            if(pitch<>0):
                if(roll>0):
                    # turn right
                    motor_L=pitch
                    motor_R=-pitch-roll/2

                else:
                    # turn left
                    motor_R=-pitch
                    motor_L=pitch+roll/2

            elif(roll<>0):
                #just rotate
                motor_R=motor_L=roll;

            else:
                # does nothing
                motor_R=motor_L=0

            if((motor_R<>0)or(motor_L<>0)):

                if(motor_R<0):
                    duty=str(hex(int(-motor_R)))
                    command_A=DRIVE_A+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_R)))
                    command_A=DRIVE_A+"01"+duty[2:]

                if(motor_L<0):
                    duty=str(hex(int(-motor_L)))
                    command_B=DRIVE_B+"00"+duty[2:]                    
                else:
                    duty=str(hex(int(motor_L)))
                    command_B=DRIVE_B+"01"+duty[2:]

                #send motors commands to SBrick
                call(command_A, shell=True);
                call(command_B, shell=True);
                sleep(0.1)

                #send COAST commands to SBrick
                call(COAST_A, shell=True);
                call(COAST_B, shell=True);

    else:
        # inactive
        sleep(0.01)

SBrick beta testing

At SBrick (or SmartBrick) Kickstarter campaign I pledged for beta tester. My beta SBrick arrived at last from Hungary:

As it’s still an early version it has some issues (one of  the four channels seems  to be damaged and LEGO Power Function cables don’t fit tight to another channel’s plug) but it’s good enough for testing connectivity with Linux, particularly with ev3dev – my main goal as beta tester is to help in connecting LEGO Mindstorms EV3 to the Sbrick.

SBrick exposes 6 Bluetooth 4.0 Low Energy «services» (I am not familiar with the BLE jargon yet): Generic Access, Device Information and another 4 specific from the vendor. The first and second services expose this information:

Device Name = SBrick
Appearance = Generic Remote Control
Model Number = 4.0
Firmware Revision = 4.1
Hardware Revision = 4.0
Software Revision = 4.1
Manufacturer Name String = Vengit Ltd.

The other 4 services are specific from Vengit and expose a total of 8 «fields»:
– 5 Read Only
– 1 Write Only
– 1 Read/Write
– 1 Unknown

Meanwhile I got from Vengit the minimal information needed for controlling a motor connected to one of the 4 channels: the reomote control service UUID is ‘4dc591b0-857c-41de-b5f1-15abda665b0c’ and the remote control characteristic is ‘2b8cbcc-0e25-4bda-8790-a15f53e6010f’.

For practical uses, using the ‘gatttool’ command from BlueZ 5 (the Linux Bluetooth stack), the above information translates to writing to handle 0x0025 the commands supported by the SBrick firmware.

I only know two of those commands (00h = BRAKE e 01h = DRIVE)

  • BRAKE Channel
  • DRIVE Channel Direction DutyCycle

‘Channel’ is one of the 4 output ports available and can be 00h, 01h, 02h or 03h.

‘Direction’ can be clockwise (00h) or anticlockwise (01h).

‘DutyCycle’ is the power pretended for the motor, can go from 00h (none or “Coast”) to FFh (“full power”).

So to send a command from Linux (a Ubuntu PC, a Raspberry Pi with Raspbian or a Mindstorms EV3 with ev3dev) with a USB BT4.0/BLE dongle one just need to use the ‘gatttool’:

$ gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=010000FF

The exemple above, sends to the Bluetooth device with address ’00:07:80:7F:28:E1′ (my SBrick) from the BT controller ‘hci0’ (my dongle) the command

DRIVE Channel#0 Clockwise 100%

As this command doesn’t keep the bluetooth connection open, the motor spins for around  3 secondss, then the connection drops and it stops (but if we use ‘gatttool’ in interactive mode with option “-I” or “–interactive” and send the equivalent commands, the motor will keep spinning).

Next video shows 2 motors spinning in opposite directions:

  • DRIVE Channel#0 Clockwise 22%
  • DRIVE Channel#2 Anticlockwise 17%

 

Next video shows an EV3 motor actiing as the reference for a Power Functionn Servo:

And a little better version of the same example, with a scale factor for matching the motor positions and a limitation of range to [-90º,+90º].

 

For that last video it was used this python script:

import traceback
from sys import exit
from time import sleep
from subprocess import call
from subprocess import check_output
from math import trunc

def reset_ev3motor():
  call ("echo 0 > /sys/class/tacho-motor/motor0/position", shell=True);
  return;

def rd_ev3motor():

  v=check_output("cat /sys/class/tacho-motor/motor0/position",shell=True);
  return(trunc(float(v)));

def sbrick_drive(channel,direction,dutycycle):
  " calls gatttool command from BlueZ, the official Linux Bluetooth protocol stack"

  if(dutycycle > 255):
    dt_hexa="0xFF";
  else:
    dt_hexa=str(hex(int(dutycycle)));

  command="gatttool -b 00:07:80:7F:28:E1 -i hci0 --char-write --handle=0x0025 --value=01"+channel+direction+dt_hexa[2:];
#  print(command);
  call (command, shell=True);

  return;

def main():
  try:
    pos=0.0;
    SCALE=255.0/90.0;

    reset_ev3motor();

    while(True):
      pos=rd_ev3motor()*SCALE;

      if(pos>0):
        sbrick_drive("02","00",pos);
      else:
        sbrick_drive("02","01",-pos)

      sleep(0.1);

  except (KeyboardInterrupt, SystemExit):
    print "Exiting...";
  except Exception:
    traceback.print_exc(file=sys.stdout);

  exit(0);

if __name__ == "__main__":
  main()

I use the ‘call’ function from library ‘subprocess’ to get to ‘gatttool’. It’s an ugly trick but it works, I did’t find a python bluetooth BLE library yet.