A few days ago the ev3dev project launched a great feature: nightly image builds. Right after that I got a received a notice that they included in the image for Raspberry Pi 2/3 support for onboard the Bluetooth and needed to test it.
So I did test it. And found out that onboard Bluetooth indeed works… as also onboard Wi-Fi… as also the Brick Pi, no need to disable BT. Yeah, no more USB dongles!
The procedure is very simple – the really important step is freeing the hardware serial port for the BrickPi (both the onboard Bluetooth and the BrickPi need a UART so a soft UART (“miniuart”) is used for BT instead of the default one.
get the latest nightly image build for the Pi2/Pi3 (mine was 26 July 2016) and restore it to a microSD card
insert the card in the Pi3
connect an USB keyboard and a HDMI display to the Pi3
power up the Pi
login (robot + maker) – if you cannot see the login prompt change to the proper console with Alt+F1 or Alt+F2 or Alt+F[n]
run ‘sudo connmanctl’ to configure BT and Wi-Fi (see this tutorial on how to configure Wi-Fi from command line; for BT just run ‘sudo connmanctl enable bluetooth’)
edit the ‘/boot/flash/config.txt’ and uncomment these 4 lines:
dtoverlay=brickpi
init_uart_clock=32000000
dtoverlay=pi3-miniuart-bt
core_freq=250
sudo reboot
remove the display and the keyboard and from now on just connect through Wi-Fi
To test that both Bluetooth and the BrickPi work properly I used a python script to read the NXT ultrasonic sensor (in the first input port) and change the color of my WeDo 2.0 Smart Hub from green to red:
#!/usr/bin/python
# run with sudo
# assumes NXT Ultrasonic at INPUT #1
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"
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])) # or "\x06\x04\x03"
DELAY = 0.3
# DO NOT FORGET TO CONFIG FOR US sensor:
# sudo echo nxt-i2c > /sys/class/lego-port/port0/mode
# sudo echo "lego-nxt-us 0x01" > /sys/class/lego-port/port0/set_device
#
us = UltrasonicSensor('ttyAMA0:S1:i2c1')
assert us.connected
req = GATTRequester(WeDo2HubAddress,True,BTdevice)
sleep(DELAY)
# configure RBG LED to Absolute Mode (accepts 3 bytes for RGB instead of default Index Mode)
req.write_by_handle(InputCommand_hnd,RGBAbsoluteMode_cmd)
while(True):
if (us.value() < 10):
print("!")
req.write_by_handle(OutputCommand_hnd, RGBAbsoluteOutput_cmd+chr(255)+chr(0)+chr(0))
sleep(DELAY)
else:
print("-")
req.write_by_handle(OutputCommand_hnd, RGBAbsoluteOutput_cmd+chr(0)+chr(255)+chr(0))
sleep(DELAY)
My script need the gattlib library to talk with Bluetooth Low Energy devices. You can install this library with ‘pip’ but first need to install some dependencies:
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))
Great news – LEGO Eduction released the WeDo 2.0 SDK today!
After digging into it, I found the information needed to control the Piezo: as expected, it’s controlled by the same handle that is used for controlling the motor and the RGB LED (0x003d). The “port” is “05” and the “command” to activate the Piezo is “02”, followed by a payload of “04” bytes containing:
the Frequency in Hz (2 bytes, reversed)
the duration in ms (2 bytes, reversed)
So to play a “C” (or “Do”, 261 Hz) during 1/8 of a second (125 ms) we use this command:
This post tries to gather all the information I collected in the last weeks related to the WeDo 2.0.
I’m not a programmer but I’m a very stubborn guy so after I managed to get my linux systems (my Ubuntu laptop, some Raspberry Pi’s and my two Mindstorms EV3) controlling the SBrick I told to myself: I’m gonna make the same with this new WeDo 2.0 system no matter what it takes.
So first things first: let’s use my Android mobile to inspect the WeDo 2.0 Hub. Nordic has a very good (and free) app that I like to use: nRF Master Control Panel (recently renamed to nRF Connect). After connecting to the Hub we find 6 services:
Some of this services, like “Battery Service” are known BLE services so [hopefully] it will be easy to use.
There’s also a very important finding at the top:
“U0001 A0:E6:F8:1E:58:57
“A0:E6:F8:1E:58:57” is the Bluetooth address of my Hub (similar to the MAC Address of every network device). It will be used *a lot* in the rest of this post.
And “U0001” is the friendly name that the Hub advertises – that’s the name that shows up in the LEGO Education WeDo 2.0 App when connecting to the Hub:
A note about this name: before I finally managed to make the LEGO Education WeDo 2.0 App work in my Android phone, my Hub advertised itself as “LPF2 Smart Hub 2 I/O”. So the LEGO App changed it to “u0001”, probably at the first time it connected to it (but since my Hub was first used by 3 other AFOL’s at Paredes de Coura I’m not sure if the process is automatic or the user is given some kind of option).
So the default (factory set) name of the Hub is “LPF2 Smart Hub 2 I/O” – LEGO Power Functions 2 Smart Hub 2 I/O”. Not much to speculate here: LEGO announced that Power Functions and Mindstorms will adopt a new plug type so new devices are expected, this is just the first one. But “Smart Hub 2 I/O” is interesting… does that means that there will be other Smart Hubs? Perhaps even a “Smart Hub 4 I/O”? That would answer some of the points I have been discussing with Fernando Conchas like “what’s the use for a 4.5 Volt device in LEGO Technic unless there’s also another device with better power features just waiting to come out”?
Now let’s look deeper to those BLE services…
I can use the nRF App and take a lot of screenshots but now that I know the BT address I will switch to my Ubuntu laptop and use 2 of the available BlueZ (the native Linux Bluetooth stack) functions, ‘hcitool’ and ‘gatttool’
The ‘gatttool’ command is a powerfull tool for BLE – in the past, without a proper BLE library for python, I used it (through python system calls) to talk with the SBrick. Clumsy but… hey, I said I’m not a programmer 😉
The ‘gatttool’ can run in an interactive mode that allows us to establish a connection and keep it until we disconnect instead of making a new connection each time we want to test a command:
$ sudo gatttool -i hci0 -I
[ ][LE]> connect A0:E6:F8:1E:58:57
Attempting to connect to A0:E6:F8:1E:58:57
Connection successful
[A0:E6:F8:1E:58:57][LE]>
In this “interactive” session we just send ‘primary’ to get the same output as using the command with ‘–primary’ option [but sometimes the commands differ a bit, so use ‘help’ and ‘–help’ to know what to use.
So the ‘primary’ command gets a list of the primary services offered by the WeDo 2.0 Hub. Of course, those are the same 6 services found by the Nordic app but that screenshot looks much better as Nordic developers added lots of intelligence to it.
So this service has 7 handles assigned (from 0x0001 to 0x0007) and serves a well know service (the ‘Generic Access‘) so it’s UUID is shortened to just 0x1800 instead of ‘00001800-0000-1000-8000-00805f9b34fb’.
Bluetooth specification for ‘Generic Access’ defines 5 properties:
From these list, only ‘Device Name’ and ‘Appearance’ are defined as ‘Mandatory’.
With the gatttool we read these properties with command ‘char-read-uuid’:
[A0:E6:F8:1E:58:57][LE]> char-read-uuid 0x2A00
handle: 0x0003 value: 75 30 30 30 31
[A0:E6:F8:1E:58:57][LE]> char-read-uuid 0x2A01
handle: 0x0005 value: 00 00
[A0:E6:F8:1E:58:57][LE]> char-read-uuid 0x2A02
Error: Read characteristics by UUID failed: No attribute found within the given range
[A0:E6:F8:1E:58:57][LE]> char-read-uuid 0x2A03
Error: Read characteristics by UUID failed: No attribute found within the given range
[A0:E6:F8:1E:58:57][LE]> char-read-uuid 0x2A04
handle: 0x0007 value: 50 00 a0 00 00 00 e8 03
So ‘Peripheral Privacy Flag’ and ‘Reconnection Address’ were not implemented by LEGO. But what’s the meaning of this hexadecimal values?
‘Device Name‘ is a string so we just convert it to ASCII (we can use an online tool like RapidTables):
75 30 30 30 31 -> u0001
Ah-ha!
‘Appearance‘ is two-byte value “composed of a category (10-bits) and sub-categories (6-bits)” that classifies the device. Since it is ‘0’, it’s not classified in any known category (so it is ‘Unknown’).
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.
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.
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).
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)
Na campanha Kickstarter do SBrick (ou SmartBrick) inscrevi-me como beta tester. O meu SBrick chegou finalmente esta semana, vindo da Hungria numa encomenda verdadeiramente espartana, sem qualquer folheto ou instruções:
É ainda uma versão muito tosca e com alguns defeitos (um dos quatro canais parece estar avariado e a ficha de outro não encaixa com firmeza nos cabos Power Functions) mas já permite ensaiar a conectividade com o Linux e em especial com o ev3dev – o meu propósito como beta tester é sobretudo ajudar na ligação entre o Mindstorms EV3 e o Sbrick.
O SBrick expõe 6 serviços Bluetooth BT4.0/BLE: Generic Access, Device Information e mais 4 específicos do fabricante. Os 2 primeiros expõem esta informação:
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.
Os outros 4 serviços são específicos da Vengit e expõem ao todo 8 campos:
– 5 Read Only
– 1 Write Only
– 1 Read/Write
– 1 Unknown
Entretanto recebi de um dos engenheiros da Vengit a informação mínima mas suficiente para controlar um motor ligado a um dos 4 canais: o UUID do serviço de controlo remoto é ‘4dc591b0-857c-41de-b5f1-15abda665b0c’, sendo a caracteristica do controlo remoto ‘2b8cbcc-0e25-4bda-8790-a15f53e6010f’.
Isso em termos práticos, utilizando o comando gatttool que vem com o BlueZ 5 (o stack de Bluetooth para Linux) corresponde a escrever no handle 0x0025 os comandos reconhecidos pelo firmware.
Até agora apenas me foram apresentados dois comandos (00h = BRAKE e 01h = DRIVE)
BRAKE Channel
DRIVE Channel Direction DutyCycle
Channel corresponde a uma das 4 portas disponíveis, podendo ser 00h, 01h, 02h ou 03h.
Direction corresponde ao sentido da rotação, podendo ser clokwise (00h) ou anticlockwise (01h).
DutyCycle corresponde à potência transmitida ao motor, variando entre 00h (“Coast”) e FFh (full power).
Assim para enviar um comando a partir de um sistema Linux (PC com Ubuntu, Raspberry Pi com Raspbian ou Mindstorms EV3 com ev3dev) equipado com dongle USB BT4.0/BLE basta invocar o comando gatttool:
No exemplo acima é enviado para o dispositivo BT com identificativo 00:07:80:7F:28:E1 (o meu SBrick) a partir do device hci0 (o meu dongle BT4.0/BLE) o comando
DRIVE Channel#0 Clockwise 100%
Como este comando não mantém a sessão bluetooth aberta, ao fim de cerca de 3 segundos ela fecha e os canais deixam de comandar os motores (se invocarmos o comando gatttool em modo interactivo com a opção “-I” ou “–interactive” e dermos a partir daí os comandos equivalentes a sessão persiste e os motores continuam activos indefinidamente).
No video abaixo são mostrados dois motores a rodar em sentidos diferentes:
DRIVE Channel#0 Clockwise 22%
DRIVE Channel#2 Anticlockwise 17%
No próximo video é mostrado um motor EV3 a servir de referência a um motor Servo Power Functions:
E finalmente uma versão um pouco mais refinada do mesmo exemplo com introdução de um factor de escala de modo a que os movimentos sejam equiparáveis e ainda uma limitação ao intervalo [-90º,+90º]
Para este último video foi usado o script abaixo, em python:
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()
Recorro à função ‘call’ da library ‘subprocess’ para invocar o comando gatttool. É feio mas funciona, ainda não tive tempo de procurar uma library específica para bluetooth BLE.
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’:
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.
Apresento o meu script em python para controlar o SBrick com um gamepad a partir do Linux. Recorro à biblioteca PyGame para ler o gamepad (assumindo que o gamepad é suportado nativamente pelo Linux, ver também o meu artigo sobre como utiizar um gamepad com ev3dev) e ao comando gatttool do BlueZ 5.0 para comunicar via Bluetooth BLE com o SBrick (assumindo também a presença de um dongle Bluetooth 4.0).
Este script funciona bem com Ubuntu mas deverá também funcionar em qualquer variante de Debian incluindo Raspbian (no Raspberry Pi) e ev3dev (no LEGO Mindstorms EV3, onde utilizei uma versão inicial deste script).
#!/usr/bin/env python
# sudo apt-get install python-pygame
import sys, traceback, os
os.environ['SDL_VIDEODRIVER'] = 'dummy'
from math import log10
from subprocess import call
from time import sleep
from pygame import joystick, event, display
### buttons ###
B_TRIANG = 0
B_CIRC = 1
B_CROSS = 2
B_SQUARE = 3
B_LTRIG2 = 4
B_RTRIG2 = 5
B_LTRIG = 6
B_RTRIG = 7
B_SELECT = 8
B_LJOY = 10
B_RJOY = 11
B_START = 9
def main():
try:
display.init();
joystick.init();
js=joystick.Joystick(0);
js.init();
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"
### starts in Joystick mode ###
control_by_JOYSTICK=True;
num_axes=js.get_numaxes();
num_buttons=js.get_numbuttons();
num_hats=js.get_numhats();
### assuming 4 axes, 13 buttons and 1 hat
flag=False;
while True:
x=y=motor_r=motor_l=0.0;
event.pump();
button_mode=js.get_button(B_SELECT);
button_shot=js.get_button(B_SQUARE);
if button_mode ==1:
if control_by_JOYSTICK==True:
control_by_JOYSTICK=False;
print 'Control Mode=HAT';
else:
control_by_JOYSTICK=True;
print 'Control Mode=JOYSTICK';
### joysticks axis [-1, +1]
### x=axis2 , y=-axis3
### ignore less than 0.2 (dead zone)
### apply log10(100x) (to reforce lower values)
### result is less or equal than 2 = log10(100)
if control_by_JOYSTICK==True:
# Control by Right Joystick, Axis 2 e 3
axis2=js.get_axis(2);
axis3=js.get_axis(3);
if axis2>0:
if axis2<0.2:
x=0;
else:
x=log10(axis2*100);
elif axis2<0:
if axis2>-0.2:
x=0;
else:
x=-log10(-axis2*100);
else:
x=0;
if axis3>0:
if axis3<0.2:
y=0;
else:
y=-log10(axis3*100);
elif axis3<0:
if axis3>-0.2:
y=0;
else:
y=log10(-axis3*100);
else:
y=0;
if y<>0:
if x<0:
motor_r=100*y;
# turn left => slow motor_l
motor_l=y*(100+25*x);
else:
motor_el=100*y;
# turn right => slow motor_r
motor_r=y*(100-25*x);
elif x<>0:
# y=0, just turn
motor_l=100*x;
motor_r=-motor_l;
else:
# Control by HAT keys
hat=js.get_hat(0);
if hat==(0,1):
# print 'FRONT';
motor_r=100;
motor_l=100;
elif hat==(1,0):
# print 'RIGHT';
motor_l=100;
motor_r=-100;
elif hat==(0,-1):
# print 'BACK';
motor_r=-100;
motor_l=-100;
elif hat==(-1,0):
# print 'LEFT';
motor_l=-100;
motor_r=100;
elif hat==(1,1):
# print 'FRONT+RIGHT';
motor_l=100;
motor_r=50;
elif hat==(-1,1):
# print 'FRONT+LEFT';
motor_l=50;
motor_r=100;
elif hat==(-1,-1):
# print 'BACK+LEFT';
motor_l=-100;
motor_r=-50;
elif hat==(1,-1):
# print 'BACK+RIGHT';
motor_l=-50;
motor_r=-100;
# get direction and duty cycle
if (motor_l<0):
dir_l="00"
duty_l=str(hex(int(-motor_l)))
else:
dir_l="01"
duty_l=str(hex(int(motor_l)))
if (motor_r<0):
dir_r="01"
duty_r=str(hex(int(-motor_r)))
else:
dir_r="00"
duty_r=str(hex(int(motor_r)))
# command+direction+dutycyle
command_A=DRIVE_A+dir_r+duty_r[2:]
command_B=DRIVE_B+dir_l+duty_l[2:]
call(command_A, shell=True);
call(command_B, shell=True);
sleep(0.1)
# call(BREAK_A,shell=True);
# call(BREAK_B,shell=True);
call(COAST_A,shell=True);
call(COAST_B,shell=True);
# end while
except (KeyboardInterrupt, SystemExit):
print "Exiting...";
except Exception:
traceback.print_exc(file=sys.stdout);
js.quit();
joystick.quit();
display.quit();
sys.exit(0);
if __name__ == "__main__":
main()
