The LEGO WeDO tilt sensor

I’ve made some reverse-engineering of the WeDO tilt sensor:

R1 = 3k9
R2 = 10k
R3 = 6k8
R4 = 1k2

The circuit is based on Philo photos of the WeDO internals, I just completed it with my own findings.

The value of R1 is critical – this resistor is connected between C1 and Ground (0V) and it is used to identify the tilt sensor – if possible use a 1% tolerance resistor.

The other 3 resistors are not so demanding because the driver of the tilt sensor accepts a range of values for each of the 4 possible states.

So we can make our own pseudo tilt sensors modifying a Power Functions cable. Since I had enough Mindstorms RCX touch sensors, why not use 4 to create a basic gamepad with 4 direction keys?

The RCX touch sensor has an internal resistor, near 600Ω when full pressed so we have to reduce R3 and R4 to near 6k2 and 0k6.

Here is a quick demo with an EV3 and 2 WeDo USB hubs. Each hub has a Power Functions L-motor in port A and one of the hubs has the 4-key gamepad. I didn’t have the right resistors so I had to use some combinations of serial and parallel, pretty ugly but it works.

How do I use two WeDo USB hubs? Since they are both seen as the same kind of device and they don’t seem to have an individual ID, I scan for all hubs (expect to find just 2) and test for the presence of a tilt sensor.

I don’t have more hubs to test but it seems that there is no limiti to the number of WeDo hubs we can use (the software from LEGO allows only 3), we just need some kind of initialization procedure (like at the beginning of our program we ask the user to connect a tilt sensor on hub #1, then hub #2 and so on until all hubs are identified – or make a motor spin a bit and ask for confirmation).

#!/usr/bin/env python

from wedo import *
from time import sleep

listofdevices=scan_for_devices()

#check if there are two hubs
if(len(listofdevices)==2):

  # initialize both hubs
  wedohub1=WeDo(listofdevices[0])
  wedohub2=WeDo(listofdevices[1])

  #find which hub has a tilt sensor

  if (wedohub1.tilt<>None):
    usehub1=True
  else:
    usehub1=False

  # use the tilt sensor as a gamepad
  # 1 = LEFT
  # 2 = BACK
  # 3 = RIGHT
  # 4 = FRONT


  while(True):

    wedohub1.motor_a=0
    wedohub2.motor_a=0

    if(usehub1):
      command=wedohub1.tilt
    else:
      command=wedohub2.tilt

    if(command==1):
      #LEFT
      wedohub1.motor_a=100
      wedohub2.motor_a=100
    elif(command==2):
      #BACK
      wedohub1.motor_a=-100
      wedohub2.motor_a=100
    elif(command==3):
      #RIGHT
      wedohub1.motor_a=-100
      wedohub2.motor_a=-100
    elif(command==4):
      #FRONT
      wedohub1.motor_a=100
      wedohub2.motor_a=-100

    sleep(0.25)

else:
  print "Number of WeDo USB hubs found was not 2"

LEGO WeDO with Linux

After almost 2 years of procrastination I finally offered myself a LEGO WeDO USB Hub:

I just love the WeDO idea: with just a small and somewhat simple interface (the USB Hub) anyone can use a PC (nowadays a ubiquitous appliance) to control a motor or read a sensor. Great for educational uses, specially with STEM (Science, Technology, Engineering and Mathematics).

Sadly the LEGO WeDO family got much less attention than his older and bigger related, LEGO Mindstorms (at least in my country, Portugal). Although a bit less expensive than an NXT or an EV3, it’s not cheap so the EV3 seems a better investment (more pieces, more diversity, more power).

Early this year I used a Raspberry Pi with a Piface Digital board as a clumsy but much more powerful DIY alternative to the WeDO. This was in fact the trigger to start using LEGO again, event registering myself in a LUG.

This week someone opened an issue with ev3dev asking for “Any chance of adding Lego WeDo hub + motor and sensors?“. Right after I suggested wedo (a python library for WeDO) Ralph Hempel announced he was working in linux kernel support for WeDO (not just for ev3dev, could be used with other systems… like my laptop). Wow!

So I got a pair of WeDO USB hubs and a tilt sensor. And while waiting for Ralph’s work, I’m using WeDo with a variant of the wedo library: WeDoMore.

This is what you need to get WeDoMore working with Linux (both Ubuntu 14.10 and ev3dev tested, no need to use sudo in ev3dev because I’m already root): first download it and extract it, then:

sudo apt-get install python-pip
sudo pip install pyusb
cd WeDoMore-master
sudo ./setup.py install

With Ubuntu it was necessary to remove Ubuntu’s version of pyusb, much older than pypi version:

sudo apt-get remove pyusb

Now connect the WeDO USB Hub and check with ‘dmesg’ and ‘lsusb’.

On Ubuntu:

413.597110] usb 2-1.3: new low-speed USB device number 10 using ehci-pci
[  413.695628] usb 2-1.3: New USB device found, idVendor=0694, idProduct=0003
[  413.695639] usb 2-1.3: New USB device strings: Mfr=0, Product=2, SerialNumber=0
[  413.695644] usb 2-1.3: Product: LEGO USB Hub V1.00
[  413.700722] hid-generic 0003:0694:0003.0009: hiddev0,hidraw3: USB HID v1.10 Device [LEGO USB Hub V1.00] on usb-0000:00:1d.0-1.3/input0
Bus 002 Device 013: ID 0694:0003 Lego Group

On ev3dev:

[ 1263.539342] usb 1-1.2: new low-speed USB device number 4 using ohci
[ 1263.698363] hid-generic 0003:0694:0003.0001: device has no listeners, quitting
Bus 001 Device 004: ID 0694:0003 Lego Group

So Ubuntu has a better knowledge of the WeDO USB Hub than ev3dev, don’t know why.

Now let’s test it from the python shell:

~$ python
Python 2.7.8 (default, Oct 20 2014, 15:05:19)
[GCC 4.9.1] on linux2
Type "help", "copyright", "credits" or "license" for more information.
>>> from wedo import WeDo
>>> wd = WeDo()
>>> wd.motor_a=100

If you have a motor connected to port A, it shell spin (note that as WeDO gets it power from USB the motor will get only 5V instead of ~9V as with Power Functions batteries… so it will spin slower).

To stop the motor and leave the shell:

>>> wd.motor_a=0
>>> exit()

As I also have a tilt sensor I made this simple control script:

#!/usr/bin/env python

from wedo import WeDo
from time import sleep

wd = WeDo()
print("Tilt me!")
while(True):
 if(wd.tilt==2): 
   wd.motor_a=35
 elif(wd.tilt==3):
   wd.motor_a=-35
 else:
   wd.motor_a=0

IR remote control – speed and position

iConor’s second LIRC configuration file,Combo_PWM.conf, allows us to use LIRC command ‘irsend’ to control the speed of a LEGO Power Functions Motor or the position of a LEGO Power Functions Servo Motor much like the 8879 IR Speed Remote Control.

This shell script sends a sequence of 7 increasingly values then returns to zero and repeat it in the opposite direction:

#!/bin/bash
#0
irsend SEND_ONCE LEGO_Combo_PWM 400B ; sleep  1.2
#1/7
irsend SEND_ONCE LEGO_Combo_PWM 401A ; sleep  1.2
#2/7
irsend SEND_ONCE LEGO_Combo_PWM 4029 ; sleep  1.2
#3/7
irsend SEND_ONCE LEGO_Combo_PWM 4038 ; sleep  1.2
#4/7
irsend SEND_ONCE LEGO_Combo_PWM 404F ; sleep  1.2
#5/7
irsend SEND_ONCE LEGO_Combo_PWM 405E ; sleep  1.2
#6/7
irsend SEND_ONCE LEGO_Combo_PWM 406D ; sleep  1.2
#7/7
irsend SEND_ONCE LEGO_Combo_PWM 407C ; sleep  1.2
#0
irsend SEND_ONCE LEGO_Combo_PWM 4083 ; sleep  1.2
#-1/7
irsend SEND_ONCE LEGO_Combo_PWM 40F4 ; sleep  1.2
#-2/7
irsend SEND_ONCE LEGO_Combo_PWM 40E5 ; sleep  1.2
#-3/7
irsend SEND_ONCE LEGO_Combo_PWM 40D6 ; sleep  1.2
#-4/7
irsend SEND_ONCE LEGO_Combo_PWM 40C7 ; sleep  1.2
#-5/7
irsend SEND_ONCE LEGO_Combo_PWM 40B0 ; sleep  1.2
#-6/7
irsend SEND_ONCE LEGO_Combo_PWM 40A1 ; sleep  1.2
#-7/7
irsend SEND_ONCE LEGO_Combo_PWM 4092 ; sleep  1.2

There is a total of 256 key codes in the configuration file (16 different values for the red plug x 16 different values for the blue plug). I used just the first subset of 16 so the first byte (for the blue plug) is always ’40’ but it doesn’t matter because I connected both Servo and M-motor to the red plug .

The sequence of key codes is not exactly the same as in the configuration file – I reordered the last 7 key codes.

The ‘sleep 1.2’  makes the script wait 1.2 seconds between each command. It is more or less the time the IR keeps the command active (bigger values would make the Servo return to center and the M-motor stop).

Infrared Remote Control

I discover this week that it is possible to use the headphone jack to send IR signals. The intructions are for a iPhone but one can use almost any audio source… even the computer audio card! And it is already suported in LIRC (Linux Infrared Remote Control), their page even includes a simple circuit!

LIRC – audio IR transmitter

Then I also found this lego-lirc repo in GitHub from a guy nicknamed iConor. It has the source code to generate LIRC configuration files for the LEGO Power Functions Infrared protocol… and also two configurations files:

The first file works like the 8885 – PF IR Remote Control and contains the usual FORWARD / BACKWARD / HOLD / BREAK codes; the second file works like the 8879 – PF IR Speed Remote Control and contains the codes to control the speed (duty cycle) of the motors.

So… why not give it a try?

I searched for more info about ‘lego-lirc’ but found nothing so I opened an issue asking iConor permission to publish those two files (and he promptly agreed, thank you iConor!).

My laptop has Ubuntu 14.10 x64. I installed LIRC and the required Portaudio library for using the audio card:

$ sudo apt-get install lirc libportaudio0

When LIRC is being installed it asks for an IR Receiver and an IR Transmitter, I chose ‘none’ twice.

Then I copied iConor configuration files to LIRC folder:

sudo cp Combo_Direct.conf /etc/lirc/lircd.conf
sudo cp Combo_PWM.conf  /etc/lirc/lircd.conf

and edited /etc/lirc/hardware.conf to create a LEGO_Combo_Direct transmitter associated to the audio card output:

(...)
#Chosen IR Transmitter
TRANSMITTER="LEGO_Combo_Direct"
TRANSMITTER_MODULES=""
TRANSMITTER_DRIVER="audio"
TRANSMITTER_DEVICE="hw@96000"
TRANSMITTER_SOCKET=""
TRANSMITTER_LIRCD_CONF="/usr/lirc/Combo_Direct.conf"
TRANSMITTER_LIRCD_ARGS=""
(...)

My audio card accepts sampling at 96 kHz but others may require lower values like 48 kHz.

I also edited /etc/lirc/lircd.conf to include both iConor configuration files

(...)
include "/etc/lirc/Combo_Direct.conf"
include "/etc/lirc/Combo_PWM.conf"
(...)

but I’m not sure if it is really needed since hardware.conf already has the right path.

I am also not sure if it is really needed or not but somewhere between my several tries I had to create a folder for lirc where it writes 2 files (one I think is a lock, the other contains just the process id)

~$ sudo mkdir /var/run/lirc/

Now let us start the lirc daemon:

~$ sudo service lirc start

and check if it is working:

~$ pgrep lirc
1234

and what kind of transmitter does it understands:

~$ irsend LIST "" ""
irsend: LEGO_Combo_Direct
irsend: LEGO_Combo_PWM

and also  what codes are available:

$ irsend LIST LEGO_Combo_Direct ""
irsend: 000000000000010e FLOAT_FLOAT
irsend: 000000000000011f FLOAT_FORWARD
irsend: 000000000000012c FLOAT_BACKWARD
irsend: 000000000000013d FLOAT_BRAKE
irsend: 000000000000014a FORWARD_FLOAT
irsend: 000000000000015b FORWARD_FORWARD
irsend: 0000000000000168 FORWARD_BACKWARD
irsend: 0000000000000179 FORWARD_BRAKE
irsend: 0000000000000186 BACKWARD_FLOAT
irsend: 0000000000000197 BACKWARD_FORWARD
irsend: 00000000000001a4 BACKWARD_BACKWARD
irsend: 00000000000001b5 BACKWARD_BRAKE
irsend: 00000000000001c2 BRAKE_FLOAT
irsend: 00000000000001d3 BRAKE_FORWARD
irsend: 00000000000001e0 BRAKE_BACKWARD
irsend: 00000000000001f1 BRAKE_BRAKE

OK, we’re ready! Now suppose we want to make both motors spin forward:

~$ irsend SEND_ONCE LEGO_Combo_Direct FORWARD_FORWARD

Yeah! The motor(s) spin for about 1 second.

So if we want shorter runs we can send a BREAK or a FLOAT after a short time, say 0.1 second:

~$ irsend SEND_ONCE LEGO_Combo_Direct FORWARD_FORWARD; sleep 0.1; irsend SEND_ONCE LEGO_Combo_Direct FLOAT_FLOAT

I faced some strange behaviours (delays, lags) until I better read the LIRC audio page and saw this warning:

“It takes some time to set up (50 ms or so) so when no clients are connected to lircd the first transmission will have some higher latency.

A workaround for this is to keep irw running with a bash script like this:

 #!/bin/bash
 while [ true ]; do
 irw || true
 sleep 1
 done

(…)”

So i created a ‘lirc-keepalive.sh’ file, gave it execution permissions and executed in the background:

~$ chmod +x lirc-keepalive.sh
~$ ./lirc-keepalive.sh &

And it makes a REALLY BIG difference.

For anyone who might get interested in this, I suggest first trying with just a headphone or 2 common LEDs (I used a pair of red). If there is a click or a blink then use 2 IR LEDs. Mine are L-934F3C – IR Emitter 3mm 940nm (€0.25 each) and no resistor at all but if you want to play safe use a small resistor like 8Ω.

You also need a cable (just cut the cable of a old headphone set) and a soldering iron. I also used heat shrinking tube but any kind of insulation material is OK.

The required materials:

And the completed cable:

Now about the LEDs…

These LEDs are from Kingbright, their specs say

1.2V Typ @ 20mA, 940 nm, 50° angle, 10 mW/sr Typ @20 mA

I also tried a pair of TSAL6100 950nm High Power Infrared Led from Vishay:

1.35V Typ@ 100 mA, 10° angle, 170 mW/sr @100 mA

but I got more or less the same results and as their emission angle is much smaller I needed to point more carefully to the PF RC receiver. so I kept the first pair. The audio output is intended for headphone so it isn’t much powerfull and might not give enough current to notice a difference – or i just made a mistake.

My first tests achieved a respectable 3 meter range. Not bad for something so simple and costing less than €1 – now everyone can programmaticaly control their LEGO models. Kudos for LIRC people for being so open-minded and for iConor for creating this configuration files.

Next I’m going to try the same with a LEGO Mindstorms EV3 and also a Raspberry Pi.

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.