ESP32 Bluetooth Low Energy Geschwindigkeitsproblem

#include <BLEDevice.h>

#include <BLEUtils.h>

#include <BLEServer.h>

#include <BLE2902.h>//client Characteristic Configuration Descriptor

#include <QList.h>

#define SERIALNUMBER "0001"

#define HARDWAREREVISION "v1.2"

#define FIRMWAREREVISION "v2.0"

#define MANUFACTURER "myCompany"

// https://www.uuidgenerator.net/

#define SERVICE_UUID        "bcb7b7ef-ebea-470b-bf47-1c9543f9e8d9"//random 128bit uuid

#define WRITECHARACTERISTIC_UUID "41eefc86-b37c-4a17-9ee0-26fb02278c25"//random 128bit uuid

#define READCHARACTERISTIC_UUID "54dd87d0-17ad-4f4e-886c-6930a33c5b4d"//random 128bit uuid

#define BATTERYSERVICE_UUID 0x180F//standard 16bit uuid for battery service

#define BATTERYLEVEL_UUID 0x2A19//standard 16bit uuid for battery level characteristic

#define DEVICEINFORMATIONSERVICE_UUID 0x180A

#define SERIALNUMBER_UUID 0x2A25

#define FIRMWAREREVISION_UUID 0x2A26

#define HARDWAREREVISION_UUID 0x2A27

#define MANUFACTURER_UUID 0x2A29

BLEServer *pServer;

BLEAdvertising *pAdvertising;

BLEService *pDeviceInformationService;

BLECharacteristic *pSerialNumber;

BLECharacteristic *pHardwareRevision;

BLECharacteristic *pFirmwareRevision;

BLECharacteristic *pManufacturer;

BLEService *pService;

BLECharacteristic *pWriteCharacteristic;

BLECharacteristic *pReadCharacteristic;

BLEService *pBatteryService;

BLECharacteristic *pBatteryLevel;

bool bleConnected = false;//for reference in the main loop, true if client is connected

//callback function for the server

class MyServerCallbacks: public BLEServerCallbacks {

    void onConnect(BLEServer* pServer) {

      bleConnected = true;

      Serial.println("device connected");

    };

    void onDisconnect(BLEServer* pServer) {

      bleConnected = false;

      Serial.println("device disconnected");

    }

};

struct sAction

{

  int strength;

  int duration;

};

typedef struct sAction Action;

QList<Action> right_list;//list for instructions

QList<Action> left_list;//list for instructions

unsigned long right_time = 0;

unsigned long left_time = 0;

boolean right_running = false;

boolean left_running = false;

int right_last_strength = 0;

int left_last_strength = 0;

//callback function for Characteristics

class MyCharacteristicCallbacks : public BLECharacteristicCallbacks {

  public:

    void onRead(BLECharacteristic* pCharacteristic) {//characteristic has bben read

      //Serial.print("read Characteristic ");

      //Serial.println(pCharacteristic->getUUID().toString().c_str());//print characteristic

    }

    void onWrite(BLECharacteristic* pCharacteristic) {//characteristic has been written to

      //Serial.print("write Characteristic ");

      //Serial.println(pCharacteristic->getUUID().toString().c_str());//print characteristic

      //Serial.println(pCharacteristic->getValue().c_str());//print value that has been written

      String uuid = pCharacteristic->getUUID().toString().c_str();

      if (uuid.indexOf(WRITECHARACTERISTIC_UUID) != -1) {

        String instruction = pWriteCharacteristic->getValue().c_str();//convert characteristic value to string

  Serial.println("-----------------------------");

  Serial.println(instruction);

          int index1 = instruction.indexOf(",");

          int index2 = instruction.indexOf(",", index1 + 1);

          if (index1 > 0) {

          boolean right = false;

          boolean left = false;

          String side = instruction.substring(0, index1);

          if (side == "right") {

            right = true;

          }

          if (side == "left") {

            left = true;

          }

          if (side == "both") {

            right = true;

            left = true;

          }

          if (index2 > index1 + 1 && index2 != instruction.length() - 1) { //valid request   *,*,*

            int strength = instruction.substring(index1 + 1, index2).toInt();

            int duration = instruction.substring(index2 + 1).toInt();

            Action action = {.strength = strength, .duration = duration};

            if (right) {

              right_list.push_back(action);

            }

            if (left) {

              left_list.push_back(action);

            }

          }

          else {

            if (instruction.indexOf("clear") != -1) {

              if (right) {

                right_list.clear();

                right_running = false;

              }

              if (left) {

                left_list.clear();

                left_running = false;

              }

            }

            else if (instruction.indexOf("next") != 1) {

              if (right) {

                right_list.pop_front();

                right_running = false;

              }

              if (left) {

                left_list.pop_front();

                left_running = false;

              }

            }

          }

        }

      }

    }

};

void setup() {

  Serial.begin(115200);//set baudrate for serial communication

  setupBLE();

  Serial.println("Setup done");

  pinMode(14, OUTPUT);

  pinMode(16, OUTPUT);

  pinMode(25, OUTPUT);

  pinMode(26, OUTPUT);

  ledcSetup(0, 10000, 8);

  ledcAttachPin(25, 0);

  ledcSetup(1, 10000, 8);

  ledcAttachPin(26, 1);

}

//merge vibration and v_right/v_left

void loop() {

  int right_strength = 0;

  if (right_list.length() > 0) {

    right_strength = right_list.front().strength;

    if (!right_running) {

      right_time = millis() + right_list.front().duration;

      right_running = true;

    }

    else if (millis() >= right_time) {

      right_list.pop_front();

      right_running = false;

    }

  }

  int left_strength = 0;

  if (left_list.length() > 0) {

    left_strength = left_list.front().strength;

    if (!left_running) {

      left_time = millis() + left_list.front().duration;

      left_running = true;

    }

    else if (millis() >= left_time) {

      left_list.pop_front();

      left_running = false;

    }

  }

  if (right_last_strength != right_strength) {

    digitalWrite(14, right_strength);

    ledcWrite(0, right_strength);

    right_last_strength = right_strength;

  }

  if (left_last_strength != left_strength) {

    digitalWrite(16, left_strength);

    ledcWrite(1, left_strength);

    left_last_strength = left_strength;

  }

  //pReadCharacteristic->setValue("OK");//response for invalid instruction

  //pReadCharacteristic->notify();//notify the client to read the response

}

void setupBLE(void) {

  BLEDevice::init("myDevice");

  pServer = BLEDevice::createServer();

  pService = pServer->createService(SERVICE_UUID);//create custom service

  pDeviceInformationService = pServer->createService(BLEUUID((uint16_t)DEVICEINFORMATIONSERVICE_UUID));//create standard battery service

  pSerialNumber = pDeviceInformationService->createCharacteristic(//create standard battery level characteristic

                    BLEUUID((uint16_t)SERIALNUMBER_UUID),

                    BLECharacteristic::PROPERTY_READ

                  );

  pHardwareRevision = pDeviceInformationService->createCharacteristic(//create standard battery level characteristic

                        BLEUUID((uint16_t)HARDWAREREVISION_UUID),

                        BLECharacteristic::PROPERTY_READ

                      );

  pFirmwareRevision = pDeviceInformationService->createCharacteristic(//create standard battery level characteristic

                        BLEUUID((uint16_t)FIRMWAREREVISION_UUID),

                        BLECharacteristic::PROPERTY_READ

                      );

  pManufacturer = pDeviceInformationService->createCharacteristic(//create standard battery level characteristic

                    BLEUUID((uint16_t)MANUFACTURER_UUID),

                    BLECharacteristic::PROPERTY_READ

                  );

  pWriteCharacteristic = pService->createCharacteristic(//create custom characteristic

                           WRITECHARACTERISTIC_UUID,

                           BLECharacteristic::PROPERTY_WRITE

                         );

  pReadCharacteristic = pService->createCharacteristic(//create custom characteristic

                          READCHARACTERISTIC_UUID,

                          BLECharacteristic::PROPERTY_READ |

                          BLECharacteristic::PROPERTY_NOTIFY

                        );

  pBatteryService = pServer->createService(BLEUUID((uint16_t)BATTERYSERVICE_UUID));//create standard battery service

  pBatteryLevel = pBatteryService->createCharacteristic(//create standard battery level characteristic

                    BLEUUID((uint16_t)BATTERYLEVEL_UUID),

                    BLECharacteristic::PROPERTY_READ

                  );

  pReadCharacteristic->addDescriptor(new BLE2902());//add a Client Characteristic Configuration Descriptor, required to turn on notify

  pSerialNumber->setValue(SERIALNUMBER);

  pHardwareRevision->setValue(HARDWAREREVISION);

  pFirmwareRevision->setValue(FIRMWAREREVISION);

  pManufacturer->setValue(MANUFACTURER);

  pWriteCharacteristic->setValue("NULL");//initialize a standard value

  pWriteCharacteristic->setValue("NULL");//initialize a standard value

  pBatteryLevel->setValue({75});//initialize a standard battery level in %

  pServer->setCallbacks(new MyServerCallbacks());//specify callback for server

  pWriteCharacteristic->setCallbacks(new MyCharacteristicCallbacks());//specify callback for characteristic

  pReadCharacteristic->setCallbacks(new MyCharacteristicCallbacks());//specify callback for characteristic

  pDeviceInformationService->start();

  pService->start();

  pBatteryService->start();

  pAdvertising = pServer->getAdvertising();

  pAdvertising->start();

}