UriShX/BLE_server_servo.ino

## BLE_server_servo.ino
/**
 * Moving a servo connected to a ESP32 dev board on pin 13, controlled via BLE.
 * Using a 9g servo, simple connections can be made from Lolin32 board to pin 13 for control, since it is next to
 * 5V and GND pins (near battery connector, PH2). The ESP32 does not have enough power on its output pins for servos,
 * so in actual use some power sourcing must be used, as well as a transistor to buffer the control pin from the servo.
 * Control web app at: https://urishx.github.io/ESP32_fader/.
 * Features auto ranging and sync via subscription.
 * Also, outputs data to serial monitor, and uses freeRTOS queue to transfer servo position from one task to another.
 * Written by Uri Shani, May 2019.
 * MIT licensed (at least my very own original contributions).
 *
 * Makes use of ESP32Servo library: https://github.com/christophevg/ESP32Servo
  */

#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLEDevice.h>
#include <BLEAdvertising.h>
#include <BLE2902.h> // BLE notify and indicate properties
#include <ESP32Servo.h> // https://github.com/christophevg/ESP32Servo

#define SERVO_PIN 13

// using alternative constructor here, passing the pin
ESP32Servo servo(SERVO_PIN);

/** freeRTOS task handles + queue */
TaskHandle_t moveServoTask;
TaskHandle_t blinkDelayTask;
TaskHandle_t sendBLEdataTask;
QueueHandle_t queue;

/** Characteristic for digital output */
BLECharacteristic *pCharacteristicRx;
BLECharacteristic *pCharacteristicTx;
/** BLE Advertiser */
BLEAdvertising* pAdvertising;
/** BLE Service */
BLEService *pService;
/** BLE Server */
BLEServer *pServer;

// bool bleOnOff = false; // start with ble off, enable it from serail monitor
bool deviceConnected = false;
bool oldDeviceConnected = false;

// See the following for generating UUIDs:
// https://www.uuidgenerator.net/
#define SERVICE_UUID           "48696828-8aba-4445-b1d2-9fe5c3e47382" // UART service UUID
#define CHARACTERISTIC_UUID_RX_TX "7dd57463-acc5-48eb-9b7f-3052779322de"

byte deg = 180; // default servo position
const unsigned int _min = 1; // minimal servo position
const unsigned int _max = 180; // maximal servo position
// formatting mask for using up to 4 single byte values in 32 bit unsigned int
const unsigned int _mask = ((0xff << 24) | (0xff << 16) | (0xff << 8));
unsigned int sendVal = (_max << 24) | (_min << 16) | deg;

String inputString = "";         // a String to hold incoming data
boolean stringComplete = false;  // whether the string is complete

void setup() {
    Serial.begin(115200); // Start serial communication

    // setup freeRTOS queue
    queue = xQueueCreate(1, sizeof(deg));

    if (queue == NULL){
        Serial.println("Error creating queue");
    }

    // servo task
    xTaskCreatePinnedToCore(
        moveServo,
        "moveServoTask",
        2048,
        NULL,
        1,
        &moveServoTask,
        1
    );
    delay(500);

    // ble task
    xTaskCreatePinnedToCore(
    sendBLEdata,
    "sendBLEdataTask",
    2048,
    NULL,
    1,
    &sendBLEdataTask,
    1
    );
    delay(500);

    inputString.reserve(20); // malloc for serial input

    initBLE(true); // can be moved to loop() and use bleOnOff as switch

    xQueueSend(queue, &deg, 0); // set initial servo position

    Serial.printf("\n\nType numbers in the range of %i to %i to set delay period. Initial value is %i.\n", _min, _max, deg);
    Serial.println(sendVal, BIN);
}

void loop() {
  //receive from serial terminal
  //should be handled in ISR, no simple implementation in Arduino IDE for ESP32
  if (Serial.available() > 0) {
    float i;
    char inByte = (char)Serial.read();
    inputString += inByte;
    if (inByte == '\n') {
      stringComplete = true;
    }
  }

  /* Handle strings when newline arrives
     Arduino terminal adds "\r\n" to each recieved string
     ' '      space
     '\t'     horizontal tab
     '\n'     newline
     '\v'     vertical tab
     '\f'     feed
     '\r'     carriage return
  */
  if (stringComplete) {
    /** start / stop ble from serial monitor */
    // if (inputString == "start\r\n") bleOnOff = ~bleOnOff;
        if (inputString[0] >= 49 && inputString[0] <= 57) {
        byte oldSelection = deg;
        byte strToInt = inputString.toInt();
        Serial.printf("Recieved number:\t%d\n", strToInt);
        if (strToInt > _max || strToInt < _min) {
            Serial.printf("Number out of range! Select numbers in the range of %d to %d.\n", _min, _max);
        } else {
            xQueueOverwrite(queue, &strToInt);
            Serial.printf("Servo angle set to:\t%i\n", strToInt);
            sendVal = (sendVal & _mask) ^ strToInt;
        }
    }
    inputString = "";
    stringComplete = false;
  }
}

void moveServo(void * parameter) {
    byte recieveDeg = 1;
    byte saveDeg = recieveDeg;
    byte currentDeg = recieveDeg;
    const unsigned int servoDelay = 30;

    // move servo loop
    while(1) {
        // get servo desired location from queue
        xQueueReceive(queue, &recieveDeg, 100);

        // if desired position different from current position
        if (recieveDeg != saveDeg) {
            // save new location and print confirmation to serial monitor
            saveDeg = recieveDeg;
            Serial.printf("Got %i through the grapevine.\nCurrent: %i.\tSaved: %i.\n", recieveDeg, currentDeg, saveDeg);

            // if desired position higher than current one, move at once
            if (saveDeg > currentDeg) {
                currentDeg = saveDeg;
                Serial.printf("Servo position set to: %i\n", currentDeg);
                servo.write(currentDeg);
            }
            // otherwise, move in three segments (trying to lower movement speed)
            else if (saveDeg < currentDeg) {
                if (saveDeg < 120) {
                if (currentDeg > 120) {
                    currentDeg = 120;
                }
                } else currentDeg = saveDeg;
                Serial.printf("Servo position set to: %i\n", currentDeg);
                servo.write(currentDeg);
                delay(servoDelay);
                byte momentary = (saveDeg < 60) ? 60 : saveDeg;
                for (currentDeg; currentDeg > momentary; currentDeg--) {
                Serial.printf("Servo position set to: %i, with delay of: %i\n", currentDeg, servoDelay);
                servo.write(currentDeg, servoDelay);
                }
                delay(servoDelay);
                currentDeg = saveDeg;
                Serial.printf("Servo position set to: %i\n", currentDeg);
                servo.write(currentDeg);
                delay(servoDelay);
            }
        }
  }
}

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

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
      pAdvertising->start();
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
        std::string rxValue = pCharacteristic->getValue();
        char rxNum = 0;

        // get value, print it out
        if (rxValue.length() > 0) {
            Serial.println("*********");
            Serial.print("Received Value: ");
            for (int i = 0; i < rxValue.length(); i++) {
            Serial.print(rxValue[i]);
            rxNum += rxValue[i];
            }

            Serial.printf("\n%x\n", rxNum);

            Serial.println("*********");
        }

        // check if value is in range
        if (rxNum > _max || rxNum < _min) {
            // if value out of range print rejection message to serial monitor
            Serial.printf("Number out of range! Select numbers in the range of %d to %d.\n", _min, _max);
        } else {
            // if value is in range set queue to recieved value
            xQueueOverwrite(queue, &rxNum);
            // format the recieve value for BLE
            sendVal = (sendVal & _mask) ^ rxNum;
            // print acknowledgment message to serial monitor
            Serial.printf("Delay period set to:\t%d\n", rxNum);
        }
    }
};

/**
   initBLE
   Initialize BLE service and characteristic
   Start BLE server and service advertising
*/
void initBLE(bool onOff) {
  Serial.printf("initBLE core %d\n ", xPortGetCoreID());
  if (onOff) {
      if (BLEDevice::getInitialized() == 0) {
      // Initialize BLE and set output power
      BLEDevice::init("ESP32 UART Service");
      BLEDevice::setPower(ESP_PWR_LVL_P7);

      // Create BLE Server
      pServer = BLEDevice::createServer();

      // Set server callbacks
      pServer->setCallbacks(new MyServerCallbacks());

      // Create BLE Service
      pService = pServer->createService(BLEUUID(SERVICE_UUID), 20);

      // Create BLE Characteristic for reading, writing, and notifying servo position
      pCharacteristicRx = pService->createCharacteristic(
                              BLEUUID(CHARACTERISTIC_UUID_RX_TX),
                              BLECharacteristic::PROPERTY_READ |
                              BLECharacteristic::PROPERTY_WRITE |
                              BLECharacteristic::PROPERTY_NOTIFY
                            );
      pCharacteristicRx->setCallbacks(new MyCallbacks());
      pCharacteristicRx->addDescriptor(new BLE2902());
      }

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

    // Start advertising
    pAdvertising = pServer->getAdvertising();
    pAdvertising->start();
  } else if (!onOff) {
        // this is a provision for starting and stopping ble services, not fully implemented here
        pAdvertising->stop();
        pService->stop();
        // in BLEDevice.cpp: void BLEDevice::deinit(bool release_memory)
        BLEDevice::deinit(true);
  }
}

// BLE notification task
void sendBLEdata(void * parameter) {
  TickType_t xLastWakeTime;
  TickType_t xPeriod = pdMS_TO_TICKS(500);

  xLastWakeTime = xTaskGetTickCount();

    while(1) {
        // if the device is connected via BLE try to send notifications
        if (deviceConnected) {
            // format data to be sent
            pCharacteristicRx->setValue(sendVal);
            std::string asSet = pCharacteristicRx->getValue();
            String setString = "";
            Serial.print("set characteristic value as:\t");
            for (byte i = 0; i < asSet.length(); i++) {
                char stringChar = asSet[i];
                setString += stringChar;
            }
            Serial.println(setString);

            // test if notifications are enabled by client
            byte testNotify = *pCharacteristicRx->getDescriptorByUUID((uint16_t)0x2902)->getValue();

            // if enabled, send value over BLE
            if (testNotify == 1) {
                pCharacteristicRx->notify(); // Send the value to the app!
                Serial.print("*** Sent Int: \t");
                Serial.print(sendVal, BIN);
                Serial.println(" ***");
            } else {
                // else print failure message to serial monitor
                Serial.print("notify failed, value of 0x2902 descriptor:\t");
                Serial.println(testNotify, HEX);//*pCharacteristicMeas->getDescriptorByUUID((uint16_t)0x2902)->getValue(), HEX);
            }
        }
        // sleep task for 500 ms
        vTaskDelayUntil(&xLastWakeTime, xPeriod);
    }
}
	/**
	* Moving a servo connected to a ESP32 dev board on pin 13, controlled via BLE.
	* Using a 9g servo, simple connections can be made from Lolin32 board to pin 13 for control, since it is next to
	* 5V and GND pins (near battery connector, PH2). The ESP32 does not have enough power on its output pins for servos,
	* so in actual use some power sourcing must be used, as well as a transistor to buffer the control pin from the servo.
	* Control web app at: https://urishx.github.io/ESP32_fader/.
	* Features auto ranging and sync via subscription.
	* Also, outputs data to serial monitor, and uses freeRTOS queue to transfer servo position from one task to another.
	* Written by Uri Shani, May 2019.
	* MIT licensed (at least my very own original contributions).
	*
	* Makes use of ESP32Servo library: https://github.com/christophevg/ESP32Servo
	*/

	#include <BLEUtils.h>
	#include <BLEServer.h>
	#include <BLEDevice.h>
	#include <BLEAdvertising.h>
	#include <BLE2902.h> // BLE notify and indicate properties
	#include <ESP32Servo.h> // https://github.com/christophevg/ESP32Servo

	#define SERVO_PIN 13

	// using alternative constructor here, passing the pin
	ESP32Servo servo(SERVO_PIN);

	/** freeRTOS task handles + queue */
	TaskHandle_t moveServoTask;
	TaskHandle_t blinkDelayTask;
	TaskHandle_t sendBLEdataTask;
	QueueHandle_t queue;

	/** Characteristic for digital output */
	BLECharacteristic *pCharacteristicRx;
	BLECharacteristic *pCharacteristicTx;
	/** BLE Advertiser */
	BLEAdvertising* pAdvertising;
	/** BLE Service */
	BLEService *pService;
	/** BLE Server */
	BLEServer *pServer;

	// bool bleOnOff = false; // start with ble off, enable it from serail monitor
	bool deviceConnected = false;
	bool oldDeviceConnected = false;

	// See the following for generating UUIDs:
	// https://www.uuidgenerator.net/
	#define SERVICE_UUID "48696828-8aba-4445-b1d2-9fe5c3e47382" // UART service UUID
	#define CHARACTERISTIC_UUID_RX_TX "7dd57463-acc5-48eb-9b7f-3052779322de"

	byte deg = 180; // default servo position
	const unsigned int _min = 1; // minimal servo position
	const unsigned int _max = 180; // maximal servo position
	// formatting mask for using up to 4 single byte values in 32 bit unsigned int
	const unsigned int _mask = ((0xff << 24) \| (0xff << 16) \| (0xff << 8));
	unsigned int sendVal = (_max << 24) \| (_min << 16) \| deg;

	String inputString = ""; // a String to hold incoming data
	boolean stringComplete = false; // whether the string is complete

	void setup() {
	Serial.begin(115200); // Start serial communication

	// setup freeRTOS queue
	queue = xQueueCreate(1, sizeof(deg));

	if (queue == NULL){
	Serial.println("Error creating queue");
	}

	// servo task
	xTaskCreatePinnedToCore(
	moveServo,
	"moveServoTask",
	2048,
	NULL,
	1,
	&moveServoTask,
	1
	);
	delay(500);

	// ble task
	xTaskCreatePinnedToCore(
	sendBLEdata,
	"sendBLEdataTask",
	2048,
	NULL,
	1,
	&sendBLEdataTask,
	1
	);
	delay(500);

	inputString.reserve(20); // malloc for serial input

	initBLE(true); // can be moved to loop() and use bleOnOff as switch

	xQueueSend(queue, &deg, 0); // set initial servo position

	Serial.printf("\n\nType numbers in the range of %i to %i to set delay period. Initial value is %i.\n", _min, _max, deg);
	Serial.println(sendVal, BIN);
	}

	void loop() {
	//receive from serial terminal
	//should be handled in ISR, no simple implementation in Arduino IDE for ESP32
	if (Serial.available() > 0) {
	float i;
	char inByte = (char)Serial.read();
	inputString += inByte;
	if (inByte == '\n') {
	stringComplete = true;
	}
	}

	/* Handle strings when newline arrives
	Arduino terminal adds "\r\n" to each recieved string
	' ' space
	'\t' horizontal tab
	'\n' newline
	'\v' vertical tab
	'\f' feed
	'\r' carriage return
	*/
	if (stringComplete) {
	/** start / stop ble from serial monitor */
	// if (inputString == "start\r\n") bleOnOff = ~bleOnOff;
	if (inputString[0] >= 49 && inputString[0] <= 57) {
	byte oldSelection = deg;
	byte strToInt = inputString.toInt();
	Serial.printf("Recieved number:\t%d\n", strToInt);
	if (strToInt > _max \|\| strToInt < _min) {
	Serial.printf("Number out of range! Select numbers in the range of %d to %d.\n", _min, _max);
	} else {
	xQueueOverwrite(queue, &strToInt);
	Serial.printf("Servo angle set to:\t%i\n", strToInt);
	sendVal = (sendVal & _mask) ^ strToInt;
	}
	}
	inputString = "";
	stringComplete = false;
	}
	}

	void moveServo(void * parameter) {
	byte recieveDeg = 1;
	byte saveDeg = recieveDeg;
	byte currentDeg = recieveDeg;
	const unsigned int servoDelay = 30;

	// move servo loop
	while(1) {
	// get servo desired location from queue
	xQueueReceive(queue, &recieveDeg, 100);

	// if desired position different from current position
	if (recieveDeg != saveDeg) {
	// save new location and print confirmation to serial monitor
	saveDeg = recieveDeg;
	Serial.printf("Got %i through the grapevine.\nCurrent: %i.\tSaved: %i.\n", recieveDeg, currentDeg, saveDeg);

	// if desired position higher than current one, move at once
	if (saveDeg > currentDeg) {
	currentDeg = saveDeg;
	Serial.printf("Servo position set to: %i\n", currentDeg);
	servo.write(currentDeg);
	}
	// otherwise, move in three segments (trying to lower movement speed)
	else if (saveDeg < currentDeg) {
	if (saveDeg < 120) {
	if (currentDeg > 120) {
	currentDeg = 120;
	}
	} else currentDeg = saveDeg;
	Serial.printf("Servo position set to: %i\n", currentDeg);
	servo.write(currentDeg);
	delay(servoDelay);
	byte momentary = (saveDeg < 60) ? 60 : saveDeg;
	for (currentDeg; currentDeg > momentary; currentDeg--) {
	Serial.printf("Servo position set to: %i, with delay of: %i\n", currentDeg, servoDelay);
	servo.write(currentDeg, servoDelay);
	}
	delay(servoDelay);
	currentDeg = saveDeg;
	Serial.printf("Servo position set to: %i\n", currentDeg);
	servo.write(currentDeg);
	delay(servoDelay);
	}
	}
	}
	}

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

	void onDisconnect(BLEServer* pServer) {
	deviceConnected = false;
	pAdvertising->start();
	}
	};

	class MyCallbacks: public BLECharacteristicCallbacks {
	void onWrite(BLECharacteristic *pCharacteristic) {
	std::string rxValue = pCharacteristic->getValue();
	char rxNum = 0;

	// get value, print it out
	if (rxValue.length() > 0) {
	Serial.println("*********");
	Serial.print("Received Value: ");
	for (int i = 0; i < rxValue.length(); i++) {
	Serial.print(rxValue[i]);
	rxNum += rxValue[i];
	}

	Serial.printf("\n%x\n", rxNum);

	Serial.println("*********");
	}

	// check if value is in range
	if (rxNum > _max \|\| rxNum < _min) {
	// if value out of range print rejection message to serial monitor
	Serial.printf("Number out of range! Select numbers in the range of %d to %d.\n", _min, _max);
	} else {
	// if value is in range set queue to recieved value
	xQueueOverwrite(queue, &rxNum);
	// format the recieve value for BLE
	sendVal = (sendVal & _mask) ^ rxNum;
	// print acknowledgment message to serial monitor
	Serial.printf("Delay period set to:\t%d\n", rxNum);
	}
	}
	};

	/**
	initBLE
	Initialize BLE service and characteristic
	Start BLE server and service advertising
	*/
	void initBLE(bool onOff) {
	Serial.printf("initBLE core %d\n ", xPortGetCoreID());
	if (onOff) {
	if (BLEDevice::getInitialized() == 0) {
	// Initialize BLE and set output power
	BLEDevice::init("ESP32 UART Service");
	BLEDevice::setPower(ESP_PWR_LVL_P7);

	// Create BLE Server
	pServer = BLEDevice::createServer();

	// Set server callbacks
	pServer->setCallbacks(new MyServerCallbacks());

	// Create BLE Service
	pService = pServer->createService(BLEUUID(SERVICE_UUID), 20);

	// Create BLE Characteristic for reading, writing, and notifying servo position
	pCharacteristicRx = pService->createCharacteristic(
	BLEUUID(CHARACTERISTIC_UUID_RX_TX),
	BLECharacteristic::PROPERTY_READ \|
	BLECharacteristic::PROPERTY_WRITE \|
	BLECharacteristic::PROPERTY_NOTIFY
	);
	pCharacteristicRx->setCallbacks(new MyCallbacks());
	pCharacteristicRx->addDescriptor(new BLE2902());
	}

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

	// Start advertising
	pAdvertising = pServer->getAdvertising();
	pAdvertising->start();
	} else if (!onOff) {
	// this is a provision for starting and stopping ble services, not fully implemented here
	pAdvertising->stop();
	pService->stop();
	// in BLEDevice.cpp: void BLEDevice::deinit(bool release_memory)
	BLEDevice::deinit(true);
	}
	}

	// BLE notification task
	void sendBLEdata(void * parameter) {
	TickType_t xLastWakeTime;
	TickType_t xPeriod = pdMS_TO_TICKS(500);

	xLastWakeTime = xTaskGetTickCount();

	while(1) {
	// if the device is connected via BLE try to send notifications
	if (deviceConnected) {
	// format data to be sent
	pCharacteristicRx->setValue(sendVal);
	std::string asSet = pCharacteristicRx->getValue();
	String setString = "";
	Serial.print("set characteristic value as:\t");
	for (byte i = 0; i < asSet.length(); i++) {
	char stringChar = asSet[i];
	setString += stringChar;
	}
	Serial.println(setString);

	// test if notifications are enabled by client
	byte testNotify = *pCharacteristicRx->getDescriptorByUUID((uint16_t)0x2902)->getValue();

	// if enabled, send value over BLE
	if (testNotify == 1) {
	pCharacteristicRx->notify(); // Send the value to the app!
	Serial.print("*** Sent Int: \t");
	Serial.print(sendVal, BIN);
	Serial.println(" ***");
	} else {
	// else print failure message to serial monitor
	Serial.print("notify failed, value of 0x2902 descriptor:\t");
	Serial.println(testNotify, HEX);//*pCharacteristicMeas->getDescriptorByUUID((uint16_t)0x2902)->getValue(), HEX);
	}
	}
	// sleep task for 500 ms
	vTaskDelayUntil(&xLastWakeTime, xPeriod);
	}
	}