Linux – Página 2

Running ev3dev on a Raspberry Pi 3

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:

sudo apt-get install pkg-config libboost-python-dev libboost-thread-dev libbluetooth-dev libglib2.0-dev python-dev

then

sudo pip install gattlib

LEGO WeDo 2.0 – playing sound

This post is part 4 of 6 of WeDo 2.0 - reverse engineering

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:

[A0:E6:F8:1E:58:57][LE]> char-write-cmd 003d 050204B801E803

So let’s hear the very first music played by a WeDo 2.0 from a linux shell script:

#!/usr/bin/env bash

# In Ubuntu run this script with sudo
# "Imperial March on a WeDo 2.0" was inspired by https://gist.github.com/tagliati/1804108

# command: gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204

# beep(a, 500) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801F401
sleep 0.5

# beep(a, 500) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801F401
sleep 0.5

# beep(a, 500) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801F401
sleep 0.5

# beep(f, 350)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D015E01
sleep 0.35

# beep(cH, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B029600
sleep 0.15

# beep(a, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801F401
sleep 0.5

# beep(f, 350)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D015E01
sleep 0.35

# beep(cH, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B029600
sleep 0.15

# beep(a, 1000)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801E803
sleep 1.0

# beep(eH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049302F401
sleep 0.5
    
# beep(eH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049302F401
sleep 0.5

# beep(eH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049302F401
sleep 0.5

# beep(fH, 350) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204BA025E01
sleep 0.35

# beep(cH, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B029600
sleep 0.15

# beep(gS, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049F01F401
sleep 0.5    

# beep(f, 350)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D015E01
sleep 0.35

# beep(cH, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B029600
sleep 0.15

# beep(a, 1000)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801E803
sleep 1.0
 
# beep(aH, 500)   
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502047003F401
sleep 0.5

# beep(a, 350) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8015E01
sleep 0.35

# beep(a, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8019600
sleep 0.15

# beep(aH, 500)   
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502047003F401
sleep 0.5

# beep(gSH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502043E03FA00
sleep 0.25

# beep(gH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502041003FA00
sleep 0.25

# beep(fSH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204E4027D00
sleep 0.125

# beep(fH, 125) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204BA027D00
sleep 0.125  
   
# beep(fSH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204E402FA00
sleep 0.25

# delay(250)
sleep 0.25

# beep(aS, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204C701FA00
sleep 0.25

# beep(dSH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502046E02F401
sleep 0.5

# beep(dH, 250)  
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502044B02FA00
sleep 0.25

# beep(cSH, 250)  
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502042A02FA00
sleep 0.25

# beep(cH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B027D00
sleep 0.125

# beep(b, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204D2017D00
sleep 0.125

# beep(cH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B02FA00
sleep 0.25
      
# delay(250)
sleep 0.25

# beep(f, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D017D00
sleep 0.125

# beep(gS, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049F01F401
sleep 0.5

# beep(f, 375) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D017701
sleep 0.375

# beep(a, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8017D00
sleep 0.125
  
# beep(cH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B02F401
sleep 0.5

# beep(a, 375)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8017701
sleep 0.375

# beep(cH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B027D00
sleep 0.125

# beep(eH, 1000)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049302E803
sleep 1.0

# beep(aH, 500)   
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502047003F401
sleep 0.5

# beep(a, 350) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8015E01
sleep 0.35

# beep(a, 150)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B8019600
sleep 0.15

# beep(aH, 500)   
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502047003F401
sleep 0.5

# beep(gSH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502043E03FA00
sleep 0.25

# beep(gH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502041003FA00
sleep 0.25

# beep(fSH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204E4027D00
sleep 0.125
    
# beep(fH, 125) 
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204BA027D00
sleep 0.125

# beep(fSH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204E402FA00
sleep 0.25

# delay(250)
sleep 0.25

# beep(aS, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204C701FA00
sleep 0.25

# beep(dSH, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502046E02F401
sleep 0.5

# beep(dH, 250)  
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502044B02FA00
sleep 0.25

# beep(cSH, 250)  
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502042A02FA00
sleep 0.25

# beep(cH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B027D00
sleep 0.125

# beep(b, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204D2017D00
sleep 0.125

# beep(cH, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B02FA00
sleep 0.25

# delay(250)
sleep 0.25

# beep(f, 250)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D01FA00
sleep 0.25

# beep(gS, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502049F01F401
sleep 0.5 
  
# beep(f, 375)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D017701
sleep 0.375

# beep(cH, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502040B027D00
sleep 0.125
           
# beep(a, 500)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801F401
sleep 0.5

# beep(f, 375)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 0502045D017701
sleep 0.375

# beep(c, 125)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 05020405017D00
sleep 0.125

# beep(a, 1000)
gatttool -i hci0 -b A0:E6:F8:1E:58:57 --char-write-req -a 0x003d -n 050204B801E803
sleep 1.0

WeDo 2.0 – reverse engineering

This post is part 1 of 6 of WeDo 2.0 - reverse engineering

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’

$ sudo hcitool -i hci0 lescan
LE Scan ...
A0:E6:F8:1E:58:57 (unknown)
A0:E6:F8:1E:58:57 LPF2 Smart Hub 2 I/O

sudo gatttool -i hci0 -b A0:E6:F8:1E:58:57 --primary
attr handle = 0x0001, end grp handle = 0x0007 uuid: 00001800-0000-1000-8000-00805f9b34fb
attr handle = 0x0008, end grp handle = 0x000b uuid: 00001801-0000-1000-8000-00805f9b34fb
attr handle = 0x000c, end grp handle = 0x002f uuid: 00001523-1212-efde-1523-785feabcd123
attr handle = 0x0030, end grp handle = 0x003e uuid: 00004f0e-1212-efde-1523-785feabcd123
attr handle = 0x003f, end grp handle = 0x0045 uuid: 0000180a-0000-1000-8000-00805f9b34fb
attr handle = 0x0046, end grp handle = 0xffff uuid: 0000180f-0000-1000-8000-00805f9b34fb

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.

First service:

attr handle = 0x0001, end grp handle = 0x0007 uuid: 00001800-0000-1000-8000-00805f9b34fb

The Nordic app shows just:

Generic Access
UUID: 0x1800
PRIMARY SERVICE

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:

Device Name (0x2A00)
Appearance (0x2A01)
Peripheral Privacy Flag (0x2A02)
Reconnection Address (0x2A03)
Peripheral Preferred Connection Parameters (0x2A04)

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’).

‘Peripheral Preferred Connection Parameters‘ is an 8-byte value containing 4 parameters (each one is 2-byte):

Minimum Connection Interval
Maximum Connection Interval
Slave Latency
Connection Supervision Timeout Multiplier

Each value is written in reverse order so

Minimum Connection Interval =005h = 80d
Maximum Connection Interval = 000ah = 160
Slave Latency = 0
Connection Supervision Timeout Multiplier = 03e8h=1000d

According to definition, the Min/Max Connection Interval values should be multiplied by 1.25 so the range is in fact 100~200 [ms].

So let’s return to the Android and validate these:

Yes, it certainly looks good!

Next post we’ll see another well-know service: the ‘Battery Service’.

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).

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)