#include <ArduinoBLE.h>
​
void setup() {
  Serial.begin(115200);
//  while (!Serial);
​
  // begin initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
​
    while (1);
  }
​
  Serial.println("BLE Central - Receiver");
  Serial.println("Make sure to turn on the device.");
​
  // start scanning for peripheral
  BLE.scan();
}
​
void loop() {
  // check if a peripheral has been discovered
  BLEDevice peripheral = BLE.available();
​
  if (peripheral) {
    // discovered a peripheral, print out address, local name, and advertised service
    Serial.print("Found ");
    Serial.print(peripheral.address());
    Serial.print(" '");
    Serial.print(peripheral.localName());
    Serial.print("' ");
    Serial.print(peripheral.advertisedServiceUuid());
    Serial.println();
​
    // Check if the peripheral is a IMUSensor, the local name will be:
    // "IMUSensor"
//    if (peripheral.address() == "ce:c1:e8:80:f0:a0") {
    if (peripheral.localName() == "IMUSensor1") {  
      // stop scanning
      BLE.stopScan();
​
      monitorBLEperipheral(peripheral);
​
      // peripheral disconnected, start scanning again
      BLE.scan();
    }
  }
}
​
void monitorBLEperipheral(BLEDevice peripheral) {
  // connect to the peripheral
  Serial.println("Connecting ...");
  if (peripheral.connect()) {
    Serial.println("Connected");
  } else {
    Serial.println("Failed to connect!");
    return;
  }
​
  // discover peripheral attributes
  Serial.println("Discovering service 0xffe0 ...");
  if (peripheral.discoverService("11a953ef-0809-4d89-9a56-25ab22841ecd")) {
    Serial.println("Service discovered");
  } else {
    Serial.println("Attribute discovery failed.");
    peripheral.disconnect();
​
    while (1);
    return;
  }
​
  // retrieve the IMUSensorData characteristic
  BLECharacteristic IMUSensorData = peripheral.characteristic("e624a94b-4a14-41f3-ae53-ec99c6025855");
//  BLEStringCharacteristic IMUSensorData = peripheral.characteristic("2A19", BLERead | BLEWrite, 512);
  
  // subscribe to the simple key characteristic
  Serial.println("Subscribing to IMUSensorData characteristic ...");
  if (!IMUSensorData) {
    Serial.println("no IMUSensorData characteristic found!");
    peripheral.disconnect();
    return;
  } 
    else if (!IMUSensorData.canSubscribe()) {
    Serial.println("IMUSensorData characteristic is not subscribable!");
    peripheral.disconnect();
    return;
  } 
    else if (!IMUSensorData.subscribe()) {
    Serial.println("subscription failed!");
    peripheral.disconnect();
    return;
  } 
    else {
    Serial.println("Subscribed to IMUSensorData characteristic");
//    Serial.println("Press the right and left buttons on your SensorTag.");
  }
​
  while (peripheral.connected()) {
    // while the peripheral is connected
​
    // check if the value of the simple key characteristic has been updated
    if (IMUSensorData.valueUpdated()) {
//    if (IMUSensorData.written()){
      // yes, get the value, characteristic is 1 byte so use byte value
//      long value = 0 ;
//      int ReceivingArray[2] = {0,0}; 
​
// THIS SECTION CONVERTS THE RECEIVED CHARACTERISTIC FROM UNSIGNED CHAR TO STRING // 
      String str;
      int length = IMUSensorData.valueLength();
      const uint8_t* val = IMUSensorData.value();
      str.reserve(length);
​
      for (int i = 0; i<length; i++){
        str += (char)val[i];
      }
​
      // Serial.println(str);
      // int posIndex = str.indexOf("POS");
      int firstComma = str.indexOf(',');
      int secondComma = str.indexOf(',', firstComma + 1);
      int thirdComma = str.indexOf(',', secondComma + 1);
      int fourthComma = str.indexOf(',', thirdComma + 1);
      int fifthComma = str.indexOf(',', fourthComma + 1);
      int sixthComma = str.indexOf(',', fifthComma + 1);
      int seventhComma = str.indexOf(',', sixthComma + 1);
      int eighthComma = str.indexOf(',', seventhComma + 1);
      int ninthComma = str.indexOf(',', eighthComma + 1);  
      int tenthComma = str.indexOf(',', ninthComma + 1); 
      int eleventhComma = str.indexOf(',', tenthComma + 1); 
      int twelvethComma = str.indexOf(',', eleventhComma + 1); 
      int thirteenthComma = str.indexOf(',', twelvethComma + 1); 
​
​
      float acc_0 = str.substring(firstComma - 1, firstComma).toFloat();
      float acc_1 = str.substring(firstComma + 1, secondComma).toFloat();
      float acc_2 = str.substring(secondComma + 1, thirdComma).toFloat();
      float gyro_0 = str.substring(thirdComma + 1, fourthComma).toFloat();     
      float gyro_1 = str.substring(fourthComma + 1, fifthComma).toFloat(); 
      float gyro_2 = str.substring(fifthComma + 1, sixthComma).toFloat();
      float roll = str.substring(sixthComma + 1, seventhComma).toFloat();
      float pitch = str.substring(seventhComma + 1, eighthComma).toFloat();
      float yaw = str.substring(eighthComma + 1, ninthComma).toFloat();
      float x = str.substring(tenthComma + 1, eleventhComma).toFloat();
      float y = str.substring(eleventhComma + 1, twelvethComma).toFloat();
      float z = str.substring(twelvethComma + 1, thirteenthComma).toFloat();  
​
​
​
​
​
      Serial.println(str);
      Serial.print(acc_0);
      Serial.print(",");
      Serial.print(acc_1);
      Serial.print(",");
      Serial.print(acc_2);
      Serial.print(",");
      Serial.print(gyro_0);
      Serial.print(",");
      Serial.print(gyro_1);
      Serial.print(",");
      Serial.print(gyro_2);
      Serial.print(",");
      Serial.print(roll);
      Serial.print(",");
      Serial.print(pitch);
      Serial.print(",");
      Serial.print(yaw);
      Serial.print(",");
      Serial.print(x);
      Serial.print(",");
      Serial.print(y);
      Serial.print(",");
      Serial.println(z);
​
​
​
    }
  }
  
​
  Serial.println("BLE disconnected!");
}