Controlling WeDo 2.0 motor from Linux

LEGO Education released the WeDO 2.0 some months ago and I bought a kit. I returned to LEGO thanks to WeDO 1.0 and since I’m already using BLE with the SBrick, using the WeDo 2.0 seemed natural.

But LEGO Education made the Android app very restrictive and only a few Android devices are allowed to install it – I have two different BLE enabled Android phones and none complies with LEGO Education definitions. Although LEGO Education promised a SDK for the new WeDO 2.0, there’s still nothing available and since I don’t own a Windows computer or an Apple device, my WeDO 2.0 kit was rusting on the shelf, waiting for better days.

Then yesterday I found a Microsoft guy’ with some bluetooth code samples with references for the WeDO 2.0 – how ironic!

So it took me only a few hours to achieve what I think is a world premiere: a linux system controlling a LEGO WeDO 2.0 motor. And what better linux system than a LEGO Mindstorms EV3?

Next post will I’ll give more details.

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)