BLE Connection to Kidbright32 - fllay/raspberrypiBLEesp32 GitHub Wiki
To create communication between a raspberry pi (e.g. Kidbright AI bot, box) and a Kidbright32 board, we can use GATT BLE since it does not require paring process. It can only use MAC address and UUIDs to establish a link between the raspberry pi and the Kidbright32 board.
BLE protocol stack is shown below.
BLE Protocol stack
The process to establish the BLE link is shown below. When the peripheral is turn on, it will start advitise (broadcast) the services. Then, the central device (Raspberry pi) will scan for BLE broadcasting signal and it will see all BLE device name and BLE MAC address. Once the desire device name is found, we can make a connection by using the corresponding BLE MAC address. We can the check all available service UUIDs. Characteristics whithin the service are where the data is stored. They act like variables for exchnaging data. There are three types of commands to exchange data between peripherals and the central. The write command is used for sending data from central to peripheral. The read command is used for central to get data from server on-demand. The server can push data to the client by using either notify or indicate command. notify command will be used when acknownlagment is not required where as notify command is used when the client can acknownlage for received data.
Let us start with ESP32 (Kidbright32 board) code. The following scketch will create a BLE server on Kidbright32 board. User can send data using serial command on Ardiono IDE serial terminal.
ESP32 Arduino code for BLE server
/*
Video: https://www.youtube.com/watch?v=oCMOYS71NIU
Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleNotify.cpp
Ported to Arduino ESP32 by Evandro Copercini
Create a BLE server that, once we receive a connection, will send periodic notifications.
The service advertises itself as: 6E400001-B5A3-F393-E0A9-E50E24DCCA9E
Has a characteristic of: 6E400002-B5A3-F393-E0A9-E50E24DCCA9E - used for receiving data with "WRITE"
Has a characteristic of: 6E400003-B5A3-F393-E0A9-E50E24DCCA9E - used to send data with "NOTIFY"
The design of creating the BLE server is:
1. Create a BLE Server
2. Create a BLE Service
3. Create a BLE Characteristic on the Service
4. Create a BLE Descriptor on the characteristic
5. Start the service.
6. Start advertising.
In this example rxValue is the data received (only accessible inside that function).
And txValue is the data to be sent, in this example just a byte incremented every second.
*/
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include "esp_bt_device.h"#include "esp_bt_device.h"
BLEServer *pServer = NULL;
BLECharacteristic * pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
void printDeviceAddress() {
const uint8_t* point = esp_bt_dev_get_address();
for (int i = 0; i < 6; i++) {
char str[3];
sprintf(str, "%02X", (int)point[i]);
Serial.print(str);
if (i < 5){
Serial.print(":");
}
}
}
class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
class MyCallbacks: public BLECharacteristicCallbacks {
void onWrite(BLECharacteristic *pCharacteristic) {
std::string rxValue = pCharacteristic->getValue();
if (rxValue.length() > 0) {
Serial.println("*********");
Serial.print("Received Value: ");
for (int i = 0; i < rxValue.length(); i++)
Serial.print(rxValue[i]);
Serial.println();
Serial.println("*********");
}
}
};
void setup() {
Serial.begin(115200);
// Create the BLE Device
BLEDevice::init("UART Service");
// Create the BLE Server
pServer = BLEDevice::createServer();
pServer->setCallbacks(new MyServerCallbacks());
// Create the BLE Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Create a BLE Characteristic
pTxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_TX,
BLECharacteristic::PROPERTY_NOTIFY
);
pTxCharacteristic->addDescriptor(new BLE2902());
BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
CHARACTERISTIC_UUID_RX,
BLECharacteristic::PROPERTY_WRITE
);
pRxCharacteristic->setCallbacks(new MyCallbacks());
// Start the service
pService->start();
// Start advertising
pServer->getAdvertising()->start();
Serial.print("BT MAC: ");
printDeviceAddress();
Serial.println("");
Serial.println("Waiting a client connection to notify...");
}
void loop() {
/*if (deviceConnected) {
pTxCharacteristic->setValue(&txValue, 1);
pTxCharacteristic->notify();
txValue++;
delay(1000); // bluetooth stack will go into congestion, if too many packets are sent
}*/
if (deviceConnected) {
if (Serial.available()){
char ch = Serial.read();
txValue = (uint8_t) ch;
pTxCharacteristic->setValue(&txValue, 1);
pTxCharacteristic->notify();
}
}
// disconnecting
if (!deviceConnected && oldDeviceConnected) {
delay(500); // give the bluetooth stack the chance to get things ready
pServer->startAdvertising(); // restart advertising
Serial.println("start advertising");
oldDeviceConnected = deviceConnected;
}
// connecting
if (deviceConnected && !oldDeviceConnected) {
// do stuff here on connecting
oldDeviceConnected = deviceConnected;
}
}After BLE server starts, the serial terninal will show that it is waiting for connection. It can be seen that the BLE MAC address is also shown in the terminal. This MAC address will be used later in python code for making a connection. In this example, the MAC address is 8C:AA:B5:8C:B7:1A
Now we are readly for python code on Raspbery pi. The required package, bluepy , can ne install by using command sudo pip3 install bluepy. The output is somewhat similar to the following
pi@KidBrightAI02:~$ sudo pip3 install bluepy
WARNING: The directory '/home/pi/.cache/pip' or its parent directory is not owned or is not writable by the current user. The cache has been disabled. Check the permissions and owner of that directory. If executing pip with sudo, you may want sudo's -H flag.
Collecting bluepy
Downloading bluepy-1.3.0.tar.gz (217 kB)
|████████████████████████████████| 217 kB 1.8 MB/s
Building wheels for collected packages: bluepy
Building wheel for bluepy (setup.py) ... done
Created wheel for bluepy: filename=bluepy-1.3.0-cp36-cp36m-linux_aarch64.whl size=563418 sha256=0c3289f178a427d8ab76230ac7097d46e77915b56df5ea338bc6b05b6cf35d92
Stored in directory: /tmp/pip-ephem-wheel-cache-gxuwz1_h/wheels/16/43/3b/dbf93f0d5e1fb36c82d1632ae26d1e921e2a6d898f05411a58
Successfully built bluepy
Installing collected packages: bluepy
Successfully installed bluepy-1.3.0
WARNING: You are using pip version 20.3.1; however, version 21.3.1 is available.
You should consider upgrading via the '/usr/bin/python3 -m pip install --upgrade pip' command.
We start with scanning code to see all BLE devices around us. Let create a bluesman.py
from bluepy.btle import Scanner, DefaultDelegate
class ScanDelegate(DefaultDelegate):
def __init__(self):
DefaultDelegate.__init__(self)
def handleDiscovery(self, dev, isNewDev, isNewData):
if isNewDev:
print("Discovered device", dev.addr)
elif isNewData:
print("Received new data from", dev.addr)
scanner = Scanner().withDelegate(ScanDelegate())
devices = scanner.scan(10.0)
for dev in devices:
print("Device {} ({}), RSSI={} dB".format(dev.addr, dev.addrType, dev.rssi))
for (adtype, desc, value) in dev.getScanData():
print(" {} = {}".format(desc, value))We can then execute code to see the BLE devices. Note that we need sudo to run this command.
pi@KidBrightAI02:~/python$ sudo python3 blescan.py
Discovered device 8c:aa:b5:8c:b7:1a
Discovered device 50:34:fc:14:f5:38
Discovered device 1c:3c:b1:a9:55:e6
Discovered device 6c:14:4b:b4:81:63
Discovered device 36:ba:92:c1:e5:e5
Discovered device d0:03:4b:e6:b6:8a
Discovered device e8:e3:74:b8:dc:34
Discovered device ff:b3:dd:32:e4:20
Device 8c:aa:b5:8c:b7:1a (public), RSSI=-104 dB
Flags = 06
Complete Local Name = UART Service
Manufacturer = 12234556
Tx Power = eb
Device 50:34:fc:14:f5:38 (random), RSSI=-106 dB
Flags = 1a
Tx Power = 0c
Manufacturer = 4c0010070a1fa9aec2ad58
Device 1c:3c:b1:a9:55:e6 (random), RSSI=-108 dB
Flags = 1a
Manufacturer = 4c0009060305c0a8440c
Device 6c:14:4b:b4:81:63 (random), RSSI=-101 dB
Flags = 1a
Manufacturer = 4c000c0e08bfa6cd7055a7c69e3d67ca384210064a1d7edfba08
Device 36:ba:92:c1:e5:e5 (random), RSSI=-108 dB
Flags = 1a
Manufacturer = 4c0009060337c0a84409
Device d0:03:4b:e6:b6:8a (public), RSSI=-107 dB
Flags = 1a
Tx Power = 0c
Manufacturer = 4c0010050814357564
Device e8:e3:74:b8:dc:34 (random), RSSI=-105 dB
Manufacturer = 4c0012020001
Device ff:b3:dd:32:e4:20 (random), RSSI=-104 dB
Manufacturer = 4c0012020003
We will see a BLE device name UART Service and the corresponding BLE MAC address 8c:aa:b5:8c:b7:1a. Next we will create a BLE client to connect to this device using BLE MAC address of 8c:aa:b5:8c:b7:1a.
To create BLE client on the Raspberry pi, we need to set the following values for connection.
- BLE MAC Address
- Service UUID
- Characteristic UUIDs
These values need to be matched with the values in Arduino code. For this example,
- BLE MAC Address is 8C:AA:B5:8C:B7:1A
- Service UUID is 6E400001-B5A3-F393-E0A9-E50E24DCCA9E
- Characteristic UUIDs are 6E400002-B5A3-F393-E0A9-E50E24DCCA9E, 6E400003-B5A3-F393-E0A9-E50E24DCCA9E Hence, python code for BLE connection initialization is
p = btle.Peripheral("8C:AA:B5:8C:B7:1A")
# Setup to turn notifications on, e.g.
svc = p.getServiceByUUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
Complete python code is given below.
from bluepy import btle
import time
class MyDelegate(btle.DefaultDelegate):
def __init__(self):
btle.DefaultDelegate.__init__(self)
# ... initialise here
def handleNotification(self, cHandle, data):
#print("\n- handleNotification -\n")
print(data)
# ... perhaps check cHandle
# ... process 'data'
# Initialisation -------
def bleConnect(addr):
p = btle.Peripheral(addr) #NodeMCU-32S
# Setup to turn notifications on, e.g.
svc = p.getServiceByUUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
p.setDelegate( MyDelegate())
return svc
addr = "80:7D:3A:FC:57:52"
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
lasttime = time.localtime()
while True:
"""
if p.waitForNotifications(1.0):
pass #continue
print("Waiting...")
"""
nowtime = time.localtime()
if(nowtime > lasttime):
lasttime = nowtime
stringtime = time.strftime("%H:%M:%S", nowtime)
btime = bytes(stringtime, 'utf-8')
try:
ch_Tx.write(btime, True)
except btle.BTLEException:
print("btle.BTLEException1");
while True:
try:
print("trying to reconnect with " )
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
print("re-connected to ")
time.sleep(1)
break
except:
continue
This code will send date-time to the BLE peripheral (Kidbright32 board) every 1 second. Once run this code at the terminal.
pi@KidBrightAI02:~/python$ python3 bleuart.py
The serial monitor will show somthing like this.
Data can be sent to BLE client (Raspberry pi) by using the serial terminal. For example, if we put "Eru" in the serial terminal and click Send.
At the terminal, we will see the follwing.
pi@KidBrightAI02:~/python$ python3 bleuart.py
b'E'
b'r'
b'u'
b'\n'
To use BLE with ROS node.
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from kidbright_tpu.msg import tpu_objects
from bluepy import btle
import time
class MyDelegate(btle.DefaultDelegate):
def __init__(self):
btle.DefaultDelegate.__init__(self)
# ... initialise here
def handleNotification(self, cHandle, data):
#print("\n- handleNotification -\n")
print(data)
# ... perhaps check cHandle
# ... process 'data'
# Initialisation -------
def bleConnect(addr):
p = btle.Peripheral(addr) #NodeMCU-32S
# Setup to turn notifications on, e.g.
svc = p.getServiceByUUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
p.setDelegate( MyDelegate())
return svc
addr = "9C:9C:1F:C5:BF:BA"
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
def callback(data):
global ch_Tx, ch_Rx, svc
#print(data.tpu_objects)
for obj in data.tpu_objects:
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", obj.label)
try:
bleData = bytes(obj.label, 'utf-8')
ch_Tx.write(bleData, True)
except btle.BTLEException:
print("btle.BTLEException1");
while True:
try:
print("trying to reconnect with " )
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
print("re-connected to ")
time.sleep(1)
break
except:
continue
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/tpu_objects", tpu_objects, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()To establish BLE connections to multiple KidBright boards, we need to create a python file for each KidBright board and each python python will be executed concurrently on the Raspberry pi.
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from kidbright_tpu.msg import tpu_objects
from bluepy import btle
import time
import Queue
q_nav = Queue.Queue()
class MyDelegate(btle.DefaultDelegate):
def __init__(self):
btle.DefaultDelegate.__init__(self)
# ... initialise here
def handleNotification(self, cHandle, data):
#print("\n- handleNotification -\n")
print(data)
# ... perhaps check cHandle
# ... process 'data'
# Initialisation -------
def bleConnect(addr):
p = btle.Peripheral(addr) #NodeMCU-32S
# Setup to turn notifications on, e.g.
svc = p.getServiceByUUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
p.setDelegate( MyDelegate())
return svc
addr = "9C:9C:1F:C5:BF:BA"
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
def callback(data):
global ch_Tx, ch_Rx, svc
#print(data.tpu_objects)
q_nav.put(data.tpu_objects)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/tpu_objects", tpu_objects, callback)
rate = rospy.Rate(10.0)
# spin() simply keeps python from exiting until this node is stopped
while not rospy.is_shutdown():
if not q_nav.empty():
tpu_objects = q_nav.get()
for obj in tpu_objects:
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", obj.label)
try:
bleData = bytes(obj.label, 'utf-8')
ch_Tx.write(bleData, True)
except btle.BTLEException:
print("btle.BTLEException1");
while True:
try:
print("trying to reconnect with " )
svc = bleConnect(addr)
ch_Tx = svc.getCharacteristics("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")[0]
ch_Rx = svc.getCharacteristics("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")[0]
print("re-connected to ")
time.sleep(1)
break
except:
continue
rate.sleep()
if __name__ == '__main__':
listener()