balaC(MPU6886)をbluetoothで操作できない

Question

M5stackのbalaCをbluetoothで操作しようと思い、BLEのコードをBalaCのコードに追加したのですが、
Bluetoothでつながりますが、その後に、倒立振り子動作にはいらなくなります。
bluetooth接続が切れると、倒立振り子動作が復活します。どうも、notifycallbackが実行されると、loopが止まってしまうように思うのですが、どうでしょうか？どなたかご教授よろしくお願いいたします。

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>



#include <M5StickC.h>

#define LED 10
#define N_CAL1 100
#define N_CAL2 100
#define LCDV_MID 60

boolean serialMonitor=true;
boolean standing=false;
int16_t counter=0;
uint32_t time0=0, time1=0;
int16_t counterOverPwr=0, maxOvp=20;
float power, powerR, powerL, yawPower;
float varAng, varOmg, varSpd, varDst, varIang;
float gyroXoffset, gyroYoffset, gyroZoffset, accXoffset;
float gyroXdata, gyroYdata, gyroZdata, accXdata, accZdata;
float aveAccX=0.0, aveAccZ=0.0, aveAbsOmg=0.0;
float cutoff=0.1; //~=2 * pi * f (Hz)
const float clk = 0.01; // in sec,
const uint32_t interval = (uint32_t) (clk*1000);  // in msec
float  Kang, Komg, KIang, Kyaw, Kdst, Kspd;
int16_t maxPwr;
float yawAngle=0.0;
float moveDestination, moveTarget;
float moveRate=0.0;
const float moveStep=0.2*clk;
int16_t fbBalance=0;
int16_t motorDeadband=0;
float mechFactR, mechFactL;
int8_t motorRDir=0, motorLDir=0;
bool spinContinuous=false;
float spinDest, spinTarget, spinFact=1.0;
float spinStep=0.0; //deg per 10msec
int16_t ipowerL=0, ipowerR=0;
int16_t motorLdir=0, motorRdir=0; //0:stop 1:+ -1:-
float vBatt, voltAve=3.7;
int16_t punchPwr, punchPwr2, punchDur, punchCountL=0, punchCountR=0;
byte demoMode=1;


// UUID HID
static BLEUUID serviceUUID("1812");
// UUID Report Charcteristic
static BLEUUID charUUID("2a4d");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;

static BLEAdvertisedDevice *myDevice;

std::vector<BLEAdvertisedDevice*> myDevices;


static void notifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic,uint8_t *pData,size_t length, bool isNotify) {
  // ボタンを押すなどしてNotifyが実行されると呼び出される

   
}

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


   
  }

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

bool connectToServer() {

  Serial.printf("myDevices=%d\n", myDevices.size());
  for (int i = 0; i < myDevices.size(); i++) {
    Serial.print("Forming a connection to ");
    Serial.println(myDevices.at(i)->getAddress().toString().c_str());

    BLEClient *pClient = BLEDevice::createClient();
    Serial.println(" - Created client");

    pClient->setClientCallbacks(new MyClientCallback());

    // Connect to the remove BLE Server.
    pClient->connect(myDevices.at(i));  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
    Serial.println(" - Connected to server");
   

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService *pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");

    // 複数のCharacteristicを持つHIDの事は考慮されていないので
    // 複数のCharacteristicに対応した処理をする
    std::map<uint16_t, BLERemoteCharacteristic*> *pCharacteristicMap;
    pCharacteristicMap = pRemoteService->getCharacteristicsByHandle();
    // 引数なしのgetCharacteristics()は複数のCharacteristicを返してこないのでこちらを利用する
    if (pCharacteristicMap == nullptr) {
      Serial.println("pCharacteristicMap=null");
      return false;
    }

    for (auto itr = pCharacteristicMap->begin(); itr != pCharacteristicMap->end(); ++itr) {
      Serial.print("UUID: ");
      Serial.println(itr->second->getUUID().toString().c_str());
      if (itr->second->getUUID().equals(charUUID)) {
        Serial.print("CharUUID matched: ");
        Serial.println(itr->second->getUUID().toString().c_str());
        if (itr->second->canNotify()) {
          // ReportCharacteristicsはNotifyCallbackでデータを通信するため
          // Notify属性を持っているCharacteristicをすべて登録する
          Serial.print("Notify registerd: ");
          Serial.println(itr->second->getUUID().toString().c_str());
          itr->second->registerForNotify(notifyCallback);
        }
      }
    }


  }
  connected = true;
  return true;
}
/**
 * Scan for BLE servers and find the first one that advertises the service we are looking for.
 */
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
  /**
   * Called for each advertising BLE server.
   */
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

//      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      myDevices.push_back(myDevice);
      doConnect = true;
      doScan = true;

    } // Found our server
  } // onResult
};


void setup() {

  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);
  Serial.begin(115200);
  M5.begin();
  Wire.begin(0, 26); //SDA,SCL
  imuInit();
  M5.Axp.ScreenBreath(11);
  M5.Lcd.setRotation(2);
  M5.Lcd.setTextFont(4);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  resetMotor();
  resetPara();
  resetVar();
  calib1();
  #ifdef DEBUG
  debugSetup();
  #else
  setMode(true);
  #endif

 BLEDevice::init("");
 BLEScan *pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);
 
}

void loop() {
 
 checkButtonP();
  #ifdef DEBUG
  if (debugLoop1()) return;
  #endif
  getGyro();
  if (!standing) {
    dispBatVolt();
    aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
    aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
    M5.Lcd.setCursor(10,130);
    M5.Lcd.printf("%5.2f   ", -aveAccZ);
    if (abs(aveAccZ)>0.9 && aveAbsOmg<1.5) {
      calib2();
      if (demoMode==1) startDemo();
      standing=true;
    }
  }
  else {
    if (abs(varAng)>30.0 || counterOverPwr>maxOvp) {
      resetMotor();
      resetVar();
      standing=false;
      setMode(false);
    }
    else {
      drive();
    }
  }
  counter += 1;
  if (counter >= 100) {
    counter = 0;
    dispBatVolt();
    if (serialMonitor) sendStatus();
  }
  do time1 = millis();
  while (time1 - time0 < interval);
  time0 = time1;


  // 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()) {
      Serial.println("We are now connected to the BLE Server.");
    }
    else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }

  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {
   
    // String newValue = "Time since boot: " + String(millis()/1000);
    // Serial.println("Setting new characteristic value to \"" + newValue + "\"");

    // // Set the characteristic's value to be the array of bytes that is actually a string.
    // pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
  }
  else if (doScan) {
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }
 
}

void calib1() {
  calDelay(30);
  digitalWrite(LED, LOW);
  calDelay(80);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, LCDV_MID);
  M5.Lcd.print(" Cal-1  ");
  gyroYoffset=0.0;
  for (int i=0; i <N_CAL1; i++){
    readGyro();
    gyroYoffset += gyroYdata;
    delay(9);
  }
  gyroYoffset /= (float)N_CAL1;
  M5.Lcd.fillScreen(BLACK);
  digitalWrite(LED, HIGH);
}

void calib2() {
  resetVar();
  resetMotor();
  digitalWrite(LED, LOW);
  calDelay(80);
  M5.Lcd.setCursor(0, LCDV_MID);
  M5.Lcd.println(" Cal-2  ");
  accXoffset=0.0;
  gyroZoffset=0.0;
  for (int i=0; i <N_CAL2; i++){
    readGyro();
    accXoffset += accXdata;
    gyroZoffset += gyroZdata;
    delay(9);
  }
  accXoffset /= (float)N_CAL2;
  gyroZoffset /= (float)N_CAL2;
  M5.Lcd.fillScreen(BLACK);
  digitalWrite(LED, HIGH);
}

void checkButtonP() {
  byte pbtn=M5.Axp.GetBtnPress();
  if (pbtn==2) calib1(); //short push
  else if (pbtn==1) setMode(true); //long push
}

void calDelay(int n) {
  for (int i=0; i<n; i++) {
    getGyro();
    delay(9);
  }
}

void setMode(bool inc) {
  if (inc) demoMode=++demoMode%2;
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(5, 5);
  if (demoMode==0) M5.Lcd.print("Stand ");
  else if (demoMode==1) M5.Lcd.print("Demo ");
 
}

void startDemo() {
  moveRate=0.0;
  spinContinuous=false;
  spinStep=0.0*clk;
 
}

以下　略

Comments

[y_waiwai]

提示のコードの動作の説明をしましょう。
どこが倒立振子のコードでしょうか

[退会済みユーザー]

倒立振子のコードは以下です。よろしくお願いします。

//
// BalaC balancing robot (IMU:MPU6886)
// by Kiraku Labo
//
// 1. Lay the robot flat, and power on.
// 2. Wait until Gal-1 (Pitch Gyro calibration) completes.
// 3. Hold still the robot upright in balance until Cal-2 (Accel & Yaw Gyro cal) completes.
//
// short push of power button: Gyro calibration
// long push (>1sec) of power button: switch mode between standig and demo(circle)
//

#include <M5StickC.h>

#define LED 10
#define N_CAL1 100
#define N_CAL2 100
#define LCDV_MID 60

boolean serialMonitor=true;
boolean standing=false;
int16_t counter=0;
uint32_t time0=0, time1=0;
int16_t counterOverPwr=0, maxOvp=20;
float power, powerR, powerL, yawPower;
float varAng, varOmg, varSpd, varDst, varIang;
float gyroXoffset, gyroYoffset, gyroZoffset, accXoffset;
float gyroXdata, gyroYdata, gyroZdata, accXdata, accZdata;
float aveAccX=0.0, aveAccZ=0.0, aveAbsOmg=0.0;
float cutoff=0.1; //~=2 * pi * f (Hz)
const float clk = 0.01; // in sec,
const uint32_t interval = (uint32_t) (clk*1000); // in msec
float Kang, Komg, KIang, Kyaw, Kdst, Kspd;
int16_t maxPwr;
float yawAngle=0.0;
float moveDestination, moveTarget;
float moveRate=0.0;
const float moveStep=0.2*clk;
int16_t fbBalance=0;
int16_t motorDeadband=0;
float mechFactR, mechFactL;
int8_t motorRDir=0, motorLDir=0;
bool spinContinuous=false;
float spinDest, spinTarget, spinFact=1.0;
float spinStep=0.0; //deg per 10msec
int16_t ipowerL=0, ipowerR=0;
int16_t motorLdir=0, motorRdir=0; //0:stop 1:+ -1:-
float vBatt, voltAve=3.7;
int16_t punchPwr, punchPwr2, punchDur, punchCountL=0, punchCountR=0;
byte demoMode=1;

void setup() {

pinMode(LED, OUTPUT);
digitalWrite(LED, HIGH);
Serial.begin(115200);
M5.begin();
Wire.begin(0, 26); //SDA,SCL
imuInit();
M5.Axp.ScreenBreath(11);
M5.Lcd.setRotation(2);
M5.Lcd.setTextFont(4);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
resetMotor();
resetPara();
resetVar();
calib1();
#ifdef DEBUG
debugSetup();
#else
setMode(true);
#endif

}

void loop() {

checkButtonP();
#ifdef DEBUG
if (debugLoop1()) return;
#endif
getGyro();
if (!standing) {
dispBatVolt();
aveAbsOmg = aveAbsOmg * 0.9 + abs(varOmg) * 0.1;
aveAccZ = aveAccZ * 0.9 + accZdata * 0.1;
M5.Lcd.setCursor(10,130);
M5.Lcd.printf("%5.2f ", -aveAccZ);
if (abs(aveAccZ)>0.9 && aveAbsOmg<1.5) {
calib2();
if (demoMode==1) startDemo();
standing=true;
}
}
else {
if (abs(varAng)>30.0 || counterOverPwr>maxOvp) {
resetMotor();
resetVar();
standing=false;
setMode(false);
}
else {
drive();
}
}
counter += 1;
if (counter >= 100) {
counter = 0;
dispBatVolt();
if (serialMonitor) sendStatus();
}
do time1 = millis();
while (time1 - time0 < interval);
time0 = time1;

}

void calib1() {
calDelay(30);
digitalWrite(LED, LOW);
calDelay(80);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setCursor(0, LCDV_MID);
M5.Lcd.print(" Cal-1 ");
gyroYoffset=0.0;
for (int i=0; i <N_CAL1; i++){
readGyro();
gyroYoffset += gyroYdata;
delay(9);
}
gyroYoffset /= (float)N_CAL1;
M5.Lcd.fillScreen(BLACK);
digitalWrite(LED, HIGH);
}

void calib2() {
resetVar();
resetMotor();
digitalWrite(LED, LOW);
calDelay(80);
M5.Lcd.setCursor(0, LCDV_MID);
M5.Lcd.println(" Cal-2 ");
accXoffset=0.0;
gyroZoffset=0.0;
for (int i=0; i <N_CAL2; i++){
readGyro();
accXoffset += accXdata;
gyroZoffset += gyroZdata;
delay(9);
}
accXoffset /= (float)N_CAL2;
gyroZoffset /= (float)N_CAL2;
M5.Lcd.fillScreen(BLACK);
digitalWrite(LED, HIGH);
}

void checkButtonP() {
byte pbtn=M5.Axp.GetBtnPress();
if (pbtn==2) calib1(); //short push
else if (pbtn==1) setMode(true); //long push
}

void calDelay(int n) {
for (int i=0; i<n; i++) {
getGyro();
delay(9);
}
}

void setMode(bool inc) {
if (inc) demoMode=++demoMode%2;
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setCursor(5, 5);
if (demoMode==0) M5.Lcd.print("Stand ");
else if (demoMode==1) M5.Lcd.print("Demo ");

}

void startDemo() {
moveRate=0.0;
spinContinuous=false;
spinStep=0.0*clk;

}

void resetPara() {
Kang=37.0;
Komg=0.84;
KIang=800.0;
Kyaw=4.0;
Kdst=85.0;
Kspd=2.7;
mechFactL=0.45;
mechFactR=0.45;
punchPwr=20;
punchDur=1;
fbBalance=-3;
motorDeadband=10;
maxPwr=120;
punchPwr2=max(punchPwr, motorDeadband);
}

void getGyro() {
readGyro();
varOmg = (gyroYdata-gyroYoffset); //unit:deg/sec
yawAngle += (gyroZdata-gyroZoffset) * clk; //unit:g
varAng += (varOmg + ((accXdata-accXoffset) * 57.3 - varAng) * cutoff ) * clk; //complementary filter
}

void readGyro() {
float gX, gY, gZ, aX, aY, aZ;
M5.Imu.getGyroData(&gX,&gY,&gZ);
M5.Imu.getAccelData(&aX,&aY,&aZ);
gyroYdata=gX;
gyroZdata=-gY;
gyroXdata=-gZ;
accXdata=aZ;
accZdata=aY;
}

void drive() {
#ifdef DEBUG
debugDrive();
#endif
if (abs(moveRate)>0.1) spinFact=constrain(-(powerR+powerL)/10.0, -1.0, 1.0); //moving
else spinFact=1.0; //standing
if (spinContinuous) spinTarget += spinStep * spinFact;
else {
if (spinTarget < spinDest) spinTarget += spinStep;
if (spinTarget > spinDest) spinTarget -= spinStep;
}
moveTarget += moveStep * (moveRate +(float)fbBalance/100.0);
varSpd += power * clk;
varDst += Kdst * (varSpd * clk -moveTarget);
varIang += KIang * varAng * clk;
power = varIang + varDst + (Kspd * varSpd) + (Kang * varAng) + (Komg * varOmg);
if (abs(power) > 1000.0) counterOverPwr += 1;
else counterOverPwr =0;
if (counterOverPwr > maxOvp) return;
power = constrain(power, -maxPwr, maxPwr);
yawPower = (yawAngle - spinTarget) * Kyaw;
powerR = power - yawPower;
powerL = power + yawPower;

ipowerL = (int16_t) constrain(powerL * mechFactL, -maxPwr, maxPwr);
int16_t mdbn=-motorDeadband;
int16_t pp2n=-punchPwr2;
if (ipowerL > 0) {
if (motorLdir == 1) punchCountL = constrain(++punchCountL, 0, 100);
else punchCountL=0;
motorLdir = 1;
if (punchCountL<punchDur) drvMotorL(max(ipowerL, punchPwr2));
else drvMotorL(max(ipowerL, motorDeadband));
}
else if (ipowerL < 0) {
if (motorLdir == -1) punchCountL = constrain(++punchCountL, 0, 100);
else punchCountL=0;
motorLdir = -1;
if (punchCountL<punchDur) drvMotorL(min(ipowerL, pp2n));
else drvMotorL(min(ipowerL, mdbn));
}
else {
drvMotorL(0);
motorLdir = 0;
}

ipowerR = (int16_t) constrain(powerR * mechFactR, -maxPwr, maxPwr);
if (ipowerR > 0) {
if (motorRdir == 1) punchCountR = constrain(++punchCountR, 0, 100);
else punchCountR=0;
motorRdir = 1;
if (punchCountR<punchDur) drvMotorR(max(ipowerR, punchPwr2));
else drvMotorR(max(ipowerR, motorDeadband));
}
else if (ipowerR < 0) {
if (motorRdir == -1) punchCountR = constrain(++punchCountR, 0, 100);
else punchCountR=0;
motorRdir = -1;
if (punchCountR<punchDur) drvMotorR(min(ipowerR, pp2n));
else drvMotorR(min(ipowerR, mdbn));
}
else {
drvMotorR(0);
motorRdir = 0;
}
}

void drvMotorL(int16_t pwm) {
drvMotor(0, (int8_t)constrain(pwm, -127, 127));
}

void drvMotorR(int16_t pwm) {
drvMotor(1, (int8_t)constrain(-pwm, -127, 127));
}

void drvMotor(byte ch, int8_t sp) {
Wire.beginTransmission(0x38);
Wire.write(ch);
Wire.write(sp);
Wire.endTransmission();
}

void resetMotor() {
drvMotorR(0);
drvMotorL(0);
counterOverPwr=0;
}

void resetVar() {
power=0.0;
moveTarget=0.0;
moveRate=0.0;
spinContinuous=false;
spinDest=0.0;
spinTarget=0.0;
spinStep=0.0;
yawAngle=0.0;
varAng=0.0;
varOmg=0.0;
varDst=0.0;
varSpd=0.0;
varIang=0.0;
}

void sendStatus () {
Serial.print(millis()-time0);
Serial.print(" stand="); Serial.print(standing);
Serial.print(" accX="); Serial.print(accXdata);
Serial.print(" power="); Serial.print(power);
Serial.print(" ang=");Serial.print(varAng);
Serial.print(", ");
Serial.print(millis()-time0);
Serial.println();
}

void imuInit() {
M5.Imu.Init();
if (M5.Imu.imuType=M5.Imu.IMU_MPU6886) {
M5.Mpu6886.SetGyroFsr(M5.Mpu6886.GFS_250DPS); //250DPS 500DPS 1000DPS 2000DPS
M5.Mpu6886.SetAccelFsr(M5.Mpu6886.AFS_4G); //2G 4G 8G 16G
if (serialMonitor) Serial.println("MPU6886 found");
}
else if (serialMonitor) Serial.println("MPU6886 not found");
}

void dispBatVolt() {
M5.Lcd.setCursor(5, LCDV_MID);
vBatt= M5.Axp.GetBatVoltage();
M5.Lcd.printf("%4.2fv ", vBatt);
}

[退会済みユーザー]

Ble 受信のコードは以下です。

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <M5StickC.h>

// UUID HID
static BLEUUID serviceUUID("1812");
// UUID Report Charcteristic
static BLEUUID charUUID("2a4d");

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;

static BLEAdvertisedDevice *myDevice;

std::vector<BLEAdvertisedDevice*> myDevices;

static void notifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic,uint8_t *pData,size_t length, bool isNotify) {
// ボタンを押すなどしてNotifyが実行されると呼び出される

}

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

}

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

bool connectToServer() {

Serial.printf("myDevices=%d\n", myDevices.size());
for (int i = 0; i < myDevices.size(); i++) {
Serial.print("Forming a connection to ");
Serial.println(myDevices.at(i)->getAddress().toString().c_str());

BLEClient *pClient = BLEDevice::createClient();
Serial.println(" - Created client");

pClient->setClientCallbacks(new MyClientCallback());

// Connect to the remove BLE Server.
pClient->connect(myDevices.at(i)); // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
Serial.println(" - Connected to server");

// Obtain a reference to the service we are after in the remote BLE server.
BLERemoteService *pRemoteService = pClient->getService(serviceUUID);
if (pRemoteService == nullptr) {
Serial.print("Failed to find our service UUID: ");
Serial.println(serviceUUID.toString().c_str());
pClient->disconnect();
return false;
}
Serial.println(" - Found our service");

// 複数のCharacteristicを持つHIDの事は考慮されていないので
// 複数のCharacteristicに対応した処理をする
std::map<uint16_t, BLERemoteCharacteristic*> *pCharacteristicMap;
pCharacteristicMap = pRemoteService->getCharacteristicsByHandle();
// 引数なしのgetCharacteristics()は複数のCharacteristicを返してこないのでこちらを利用する
if (pCharacteristicMap == nullptr) {
Serial.println("pCharacteristicMap=null");
return false;
}

for (auto itr = pCharacteristicMap->begin(); itr != pCharacteristicMap->end(); ++itr) {
Serial.print("UUID: ");
Serial.println(itr->second->getUUID().toString().c_str());
if (itr->second->getUUID().equals(charUUID)) {
Serial.print("CharUUID matched: ");
Serial.println(itr->second->getUUID().toString().c_str());
if (itr->second->canNotify()) {
// ReportCharacteristicsはNotifyCallbackでデータを通信するため
// Notify属性を持っているCharacteristicをすべて登録する
Serial.print("Notify registerd: ");
Serial.println(itr->second->getUUID().toString().c_str());
itr->second->registerForNotify(notifyCallback);
}
}
}

}
connected = true;
return true;
}
/**
* Scan for BLE servers and find the first one that advertises the service we are looking for.
*/
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
/**
* Called for each advertising BLE server.
*/
void onResult(BLEAdvertisedDevice advertisedDevice) {
Serial.print("BLE Advertised Device found: ");
Serial.println(advertisedDevice.toString().c_str());

// We have found a device, let us now see if it contains the service we are looking for.
if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {

// BLEDevice::getScan()->stop();
myDevice = new BLEAdvertisedDevice(advertisedDevice);
myDevices.push_back(myDevice);
doConnect = true;
doScan = true;

} // Found our server
} // onResult
};

void setup() {

BLEDevice::init("");
BLEScan *pBLEScan = BLEDevice::getScan();
pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
pBLEScan->setInterval(1349);
pBLEScan->setWindow(449);
pBLEScan->setActiveScan(true);
pBLEScan->start(5, false);

}

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()) {
Serial.println("We are now connected to the BLE Server.");
}
else {
Serial.println("We have failed to connect to the server; there is nothin more we will do.");
}
doConnect = false;
}

// If we are connected to a peer BLE Server, update the characteristic each time we are reached
// with the current time since boot.
if (connected) {

// String newValue = "Time since boot: " + String(millis()/1000);
// Serial.println("Setting new characteristic value to \"" + newValue + "\"");

// // Set the characteristic's value to be the array of bytes that is actually a string.
// pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
}
else if (doScan) {
BLEDevice::getScan()->start(0); // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
}

}