ESP32 Forum

Code: Select all

// CLIENT | CENTRAL | RECEIVER (screen)
// SDA GPIO21 SCK GPIO22 for ESP32-WROOM-32

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Adafruit_SSD1306.h>

#include <BLEDevice.h>

// BLE Server name (the other ESP32 name running the server sketch)
#define RG_Server "Right Glove"
#define RG_Server "Left Glove"


// replaced upper bytes with 0s, first two bytes will be for services and second two bytes characteristics
static BLEUUID RGlove_SERVICE_UUID("5adad05b-dee6-4c49-9a60-ce8cf0fe6a19");

// configure the characteristic to understand what kind of data the server is expecting to receive from the client
// or what the client will be receiving from the server
static BLEUUID IMU_CHAR_UUID("5adad051-dee6-4c49-9a60-ce8cf0fe6a19");

// Flags stating if should begin connecting and if the connection is up
static bool doConnect = false;
static bool connected = false;

// Address of the peripheral device. Address will be found during scanning...
static BLEAddress *pServerAddress;

// Characteristicd that we want to read
static BLERemoteCharacteristic *IMU_CHAR;

// Activate notify
const uint8_t notificationOn[] = {0x1, 0x0};
const uint8_t notificationOff[] = {0x0, 0x0};

/*                  SCREEN STUFF                          */

// screen dimensions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32

int ctr = 0;

// Variables to store orientation and acceleration values
// Flags to check whether new temperature and humidity readings are available

class MyClientCallback : public BLEClientCallbacks
{
    void onConnect(BLEClient *pclient)
    {
    }

    void onDisconnect(BLEClient *pclient)
    {
        connected = false;
        Serial.println("onDisconnect");
    }
};

class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
{
    void onResult(BLEAdvertisedDevice advertisedDevice)
    {
        if (advertisedDevice.getName() == RG_Server)
        {                                                                   // Check if the name of the advertiser matches
            advertisedDevice.getScan()->stop();                             // Scan can be stopped, we found what we are looking for
            pServerAddress = new BLEAddress(advertisedDevice.getAddress()); // Address of advertiser is the one we need
            doConnect = true;                                               // Set indicator, stating that we are ready to connect
            Serial.println("Device found. Connecting!");
        }
    }
};

// Connect to the BLE Server that has the name, Service, and Characteristics
bool connectToServer(BLEAddress pAddress)
{
    BLEClient *pClient = BLEDevice::createClient();
    pClient->setClientCallbacks(new MyClientCallback()); // not sure

    // Connect to the remove BLE Server.
    pClient->connect(pAddress);
    Serial.println(" - Connected to server");

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService *pRemoteService = pClient->getService(RGlove_SERVICE_UUID);
    if (pRemoteService == nullptr)
    {
        Serial.print("Failed to find our service UUID: ");
        Serial.println(RGlove_SERVICE_UUID.toString().c_str());
        return (false);
    }
    // Obtain a reference to the characteristics in the service of the remote BLE server.
    IMU_CHAR = pRemoteService->getCharacteristic(IMU_CHAR_UUID); // STACK SMASHING FAILURE FROM HERE

    if (IMU_CHAR == nullptr)
    {

        Serial.print("Failed to find our characteristic UUID");
        return false;
    }
    Serial.println(" - Found our characteristics");

    // Assign callback functions for the Characteristics
    IMU_CHAR->registerForNotify(IMU_NotifyCallback);
    connected = true; // not sure
    return true;
}

// When the BLE Server sends a new reading with the notify property
static void IMU_NotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic,
                               uint8_t *pData, size_t length, bool isNotify)
{

    if (ctr == 0)
    {
        Serial.print("ORIENTATION X: ");
        Serial.println((char *)pData);
        ctr = ctr + 1;
    }
    else if (ctr == 1)
    {
        Serial.print("ORIENTATION Y: ");
        Serial.println((char *)pData);
        ctr = ctr + 1;
    }
    else if (ctr == 2)
    {
        Serial.print("ORIENTATION Z: ");
        Serial.println((char *)pData);
        ctr = ctr + 1;
    }
    else if (ctr == 3)
    {
        Serial.print("ACCELERATION X: ");
        Serial.println((char *)pData);
        ctr = ctr + 1;
    }
    else if (ctr == 4)
    {
        Serial.print("ACCELERATION Y: ");
        Serial.println((char *)pData);
        ctr = ctr + 1;
    }
    else if (ctr == 5)
    {
        Serial.print("ACCELERATION Z: ");
        Serial.println((char *)pData);
        ctr = 0;
    }
}

void setup()
{

    // Start serial communication
    Serial.begin(115200);
    Serial.println("Starting Arduino BLE Client application...");

    // Init BLE device
    BLEDevice::init("");

    // Retrieve a Scanner and set the callback we want to use to be informed when we
    // have detected a new device.  Specify that we want active scanning and start the
    // scan to run for 30 seconds.
    BLEScan *pBLEScan = BLEDevice::getScan();
    pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
    pBLEScan->setActiveScan(true);
    pBLEScan->start(30);
}

void loop()
{
    // If the flag "doConnect" is true then we have scanned for and found the desired
    // BLE Server with which we wish to connect.  Now we connect to it.  Once we are
    // connected we set the connected flag to be true.
    if (doConnect == true)
    {
        if (connectToServer(*pServerAddress))
        {
            Serial.println("We are now connected to the BLE Server.");

            // Activate the Notify property of each Characteristic
            IMU_CHAR->getDescriptor(BLEUUID((uint16_t)0x2902))->writeValue((uint8_t *)notificationOn, 2, true);
            Serial.println("connected");

            connected = true;
        }
        else
        {
            Serial.println("We have failed to connect to the server; Restart your device to scan for nearby BLE server again.");
        }
        doConnect = false;
    }
    // delay(1000); // Delay a second between loops.
}

Code: Select all

// SERVER | PERIPHERAL | TRANSMITTER (sensors)
// SDA GPIO21 SCK GPIO22 for ESP32-WROOM-32

// display for testing purposes only, delete when done with testing

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

#include <Adafruit_SSD1306.h>

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32

// delay between fresh samples
#define BNO055_SAMPLERATE_DELAY_MS (100)
// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
Adafruit_SSD1306 display = Adafruit_SSD1306(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

// replaced upper bytes with 0s, first two bytes will be for services and second two bytes characteristics
// BLEService

//#define RGlove_SERVICE_UUID BLEUUID((uint16_t)0x0000)
#define RGlove_SERVICE_UUID "5adad05b-dee6-4c49-9a60-ce8cf0fe6a19"

#define BLE_server "Right Glove"

// dumb values, easy to spot problem
float acc_x = -1000000, ori_x = -1000000;
float acc_y = -1000000, ori_y = -1000000;
float acc_z = -1000000, ori_z = -1000000;

float acc_xtest = 0, ori_xtest = 0;
float acc_ytest = 0, ori_ytest = 0;
float acc_ztest = 0, ori_ztest = 0;

BLECharacteristic IMU_CHAR( // x characteristic will only be read
    "5adad051-dee6-4c49-9a60-ce8cf0fe6a19",
    BLECharacteristic::PROPERTY_NOTIFY);

BLEDescriptor IMU_CHAR_descriptor(BLEUUID((uint16_t)0x2902)); // Add this line only if the characteristic has the Notify property

bool deviceConnected = false;

class MyServerCallbacks : public BLEServerCallbacks
{
    void onConnect(BLEServer *pServer)
    {
        deviceConnected = true;
    };

    void onDisconnect(BLEServer *pServer)
    {
        deviceConnected = false;
    }
};

void setup()
{
    Serial.begin(115200);

    /* Initialise the sensor */
    if (!bno.begin())
    {
        // There was a problem detecting the BNO055 ... check your connections
        Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
        while (1)
            ;
    }

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
    { // Address 0x3C for 128x64
        Serial.println(F("SSD1306 allocation failed"));
        for (;;)
            ; // Don't proceed, loop forever
    }
    display.display();
    delay(500); // Pause for 2 seconds
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setRotation(0);

    delay(1000);

    bno.setExtCrystalUse(true);

    Serial.println("Starting BLE work!");

    // Create BLE Device name
    BLEDevice::init(BLE_server);

    // create BLE Server
    BLEServer *pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());

    // Create BLE Service
    BLEService *RGlove_SERVICE = pServer->createService(RGlove_SERVICE_UUID);

    // Create BLE Characteristics
    RGlove_SERVICE->addCharacteristic(&IMU_CHAR);

    IMU_CHAR_descriptor.setValue("Orientation and Acceleration");

    /* Add Descriptors to the Characteristic*/
    IMU_CHAR.addDescriptor(new BLE2902()); // Add this line only if the characteristic has the Notify property

    // start continuously transmitting BLE advertising packets
    // and will be visible to remote BLE central devices until it receives a new new connection

    pServer->getAdvertising()->addServiceUUID(RGlove_SERVICE_UUID); // not in reference?

    // Start the service
    RGlove_SERVICE->start();

    pServer->getAdvertising()->start();

    Serial.println("Characteristic defined! Waiting a client connection to notify...");
}

void loop()
{
    sensors_event_t orientationData, accelerometerData;
    bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
    bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);

    display.clearDisplay();
    display.setCursor(0, 0);

    readOriValues(&orientationData);
    readAccValues(&accelerometerData);

    if (deviceConnected)
    {
        // int16_t value;
        delay(200);
        static char ori_x_arr[6];
        static char ori_y_arr[6];
        static char ori_z_arr[6];
        static char acc_x_arr[5];
        static char acc_y_arr[5];
        static char acc_z_arr[5];

        dtostrf(ori_x, 4, 1, ori_x_arr);
        dtostrf(ori_y, 4, 1, ori_y_arr);
        dtostrf(ori_z, 4, 1, ori_z_arr);
        dtostrf(acc_x, 3, 1, acc_x_arr);
        dtostrf(acc_y, 3, 1, acc_y_arr);
        dtostrf(acc_z, 3, 1, acc_z_arr);

        // setValue only accepts 8bit unisgned integers
        IMU_CHAR.setValue(ori_x_arr);
        IMU_CHAR.notify();
        Serial.print("ori X:");
        Serial.println(ori_x);

        IMU_CHAR.setValue(ori_y_arr);
        IMU_CHAR.notify();
        Serial.print("ori Y:");
        Serial.println(ori_y);

        IMU_CHAR.setValue(ori_z_arr);
        IMU_CHAR.notify();
        Serial.print("ori Z:");
        Serial.println(ori_z);

        IMU_CHAR.setValue(acc_x_arr);
        IMU_CHAR.notify();
        Serial.print("acc X:");
        Serial.println(acc_x);

        IMU_CHAR.setValue(acc_y_arr);
        IMU_CHAR.notify();
        Serial.print("acc Y:");
        Serial.println(acc_y);

        IMU_CHAR.setValue(acc_z_arr);
        IMU_CHAR.notify();
        Serial.print("acc Z:");
        Serial.println(acc_z);
    }

    display.setTextSize(1);
    display.print("ori");
    display.setTextSize(1.3);
    display.setCursor(0, 8);
    display.print(ori_x, 1);
    display.print(", ");
    display.print(ori_y, 1);
    display.print(", ");
    display.print(ori_z, 1);

    display.setCursor(0, 17);
    display.setTextSize(1);
    display.print("acc");

    display.setTextSize(1.3);
    display.setCursor(0, 25);
    display.print(acc_x, 1);
    display.print(", ");
    display.print(acc_y, 1);
    display.print(", ");
    display.print(acc_z, 1);

    display.display();
}

void readOriValues(sensors_event_t *event)
{
    // read orientation event
    ori_x = event->orientation.x;
    ori_y = event->orientation.y;
    ori_z = event->orientation.z;
}

void readAccValues(sensors_event_t *event)
{
    // read acceleration event
    acc_x = event->acceleration.x;
    acc_y = event->acceleration.y;
    acc_z = event->acceleration.z;
}