1. #include "BLEDevice.h"
  2. int Connect_LED = 33;
  3. int x_val, y_val, yr_val, yl_val, z_val, z_button, c_button;
  4. int RL_val, L_value, R_value, x_val2, x_val3;
  5. int Cv_r = 15; // R-Motor Correction value 補正値
  6. int Cv_l = 0; // L-Motor Correction value 補正値
  7. char* data;
  8. #define R_motor 26
  9. #define L_motor 27
  10. #define SERVICE_UUID "cdb068be-4458-11e9-b210-d663bd873d93"
  11. #define CHARACTERISTIC_UUID "cdb06c10-4458-11e9-b210-d663bd873d93"
  12. #define SERVER_NAME "esp32_BLE"
  13. static BLEUUID serviceUUID(SERVICE_UUID);
  14. static BLEUUID charUUID(CHARACTERISTIC_UUID);
  15. static BLEAddress *pServerAddress;
  16. static boolean doConnect = false;
  17. static boolean connected = false;
  18. static BLERemoteCharacteristic* pRemoteCharacteristic;
  19. static void notifyCallback(
  20.   BLERemoteCharacteristic* pBLERemoteCharacteristic,
  21.   uint8_t* pData,
  22.   size_t length,
  23.   bool isNotify) {
  24. }
  25. bool connectToServer(BLEAddress pAddress) {
  26.   //Serial.print("Forming a connection to ");
  27.   //Serial.println(pAddress.toString().c_str());
  28.   BLEClient* pClient = BLEDevice::createClient();
  29.   pClient->connect(pAddress);
  30.   BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
  31.   //Serial.print(" - Connected to server :");
  32.   //Serial.println(serviceUUID.toString().c_str());
  33.   if (pRemoteService == nullptr) {
  34.     //Serial.print("Failed to find our service UUID: ");
  35.     //Serial.println(serviceUUID.toString().c_str());
  36.     return false;
  37.   }
  38.   pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
  39.   //Serial.print(" - Found our characteristic UUID: ");
  40.   //Serial.println(charUUID.toString().c_str());
  41.   if (pRemoteCharacteristic == nullptr) {
  42.     //Serial.print("Failed to find our characteristic UUID: ");
  43.     return false;
  44.   }
  45.   //Serial.println(" - Found our characteristic");
  46.   pRemoteCharacteristic->registerForNotify(notifyCallback);
  47. }
  48. class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
  49.     void onResult(BLEAdvertisedDevice advertisedDevice) {
  50.       //Serial.print("BLE Advertised Device found: ");
  51.       //Serial.println(advertisedDevice.toString().c_str());
  52.       //Serial.println(advertisedDevice.getName().c_str());
  53.       if (advertisedDevice.getName() == SERVER_NAME) {
  54.         //Serial.println(advertisedDevice.getAddress().toString().c_str());
  55.         advertisedDevice.getScan()->stop();
  56.         pServerAddress = new BLEAddress(advertisedDevice.getAddress());
  57.         doConnect = true;
  58.       }
  59.     }
  60. };
  61. void setup() {
  62.   Serial.begin(115200);
  63.   BLEDevice::init(SERVER_NAME);
  64.   BLEScan* pBLEScan = BLEDevice::getScan();
  65.   pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  66.   pBLEScan->setActiveScan(true);
  67.   pBLEScan->start(2);
  68.   pinMode(Connect_LED, OUTPUT); digitalWrite(Connect_LED, LOW);// BLE Status LED
  69.   ledcSetup(0, 500, 8); // (0ch,500Hz,8bit resolution)
  70.   ledcAttachPin(L_motor, 0); // 0ch
  71.   ledcSetup(1, 500, 8); // (0ch,500Hz,8bit resolution)
  72.   ledcAttachPin(R_motor, 1); // 1ch
  73. }
  74. void loop() {
  75.   if (doConnect == true) {
  76.     if (connectToServer(*pServerAddress)) {
  77.       Serial.println("We are now connected to the BLE Server.");
  78.       connected = true;
  79.     } else {
  80.       Serial.println("We have failed to connect to the server; there is nothin more we will do.");
  81.       connected = false;
  82.     }
  83.     doConnect = false;
  84.   }
  85.   if (connected) {
  86.     std::string value = pRemoteCharacteristic->readValue();
  87.     String strVal = value.c_str();
  88.     char strNum = strVal.toInt();
  89.     data = new char[strVal.length() + 1];
  90.     strcpy(data, value.c_str());
  91.     if (sscanf(data, "%d,%d,%d", &x_val, &y_val, &z_val) == 3) {
  92.       if (z_val == 0)z_button = 1;
  93.       if (z_val == 1)z_button = 0;
  94.       if (z_val == 0)c_button = 1;
  95.       if (z_val == 2)c_button = 0;
  96.       if (z_val == 3) {
  97.         z_button = 0;
  98.         c_button = 0;
  99.       }
  100.      //Serial.print(" x_val , ") ; //x_val実測値:min=22,max=207,center=123
  101.      //Serial.print(x_val) ;
  102.      //Serial.print(" y_val , ") ; //y_val実測値:min=31,max=212,center=131
  103.      //Serial.println(y_val) ;
  104.       if (x_val < 113) {
  105.         x_val = map(x_val, 22, 113, 50, 1);
  106.         //実測に合わせ、X軸の入力最小値を22に設定、x_valの設定範囲を50~1とします。
  107.         RL_val = 1;
  108.       }
  109.       else if (x_val > 133) {
  110.         x_val = map(x_val, 133, 207, 1, 50);
  111.        //実測に合わせ、X軸の入力最大値を207に設定、x_valの設定範囲を1~50とします。
  112.         RL_val = 2;
  113.       } else {
  114.         x_val = 0; // X軸center値123の±10=113~133を不感帯をx_val=0とします。
  115.         RL_val = 0;
  116.       }
  117.       yr_val = map(y_val, 31, 212, 0, 255 - Cv_r); // 右モータ:補正値15
  118.       yr_val = max(yr_val, 0);
  119.       yr_val = min(yr_val, 255 - Cv_r);
  120.       yl_val = map(y_val, 31, 212, 0, 255 - Cv_l); // 左モータ:補正値0
  121.       yl_val = max(yl_val, 0);
  122.       yl_val = min(yl_val, 255 - Cv_l);
  123.       if (RL_val == 1) {
  124.         L_value = yl_val - (yl_val / 100 * x_val);
  125.         //x_valを0~50に調節すると、yl_valが0~50%に変化します。
  126.         ledcWrite(0, L_value);
  127.         delay(5);
  128.       } else {
  129.         L_value = yl_val;
  130.         ledcWrite(0, L_value);
  131.       }
  132.       if (RL_val == 2) {
  133.         R_value = yr_val - (yr_val / 100 * x_val);
  134.         //x_valを0~50に調節すると、yr_valが0~50%に変化します。
  135.         ledcWrite(1, R_value);
  136.         delay(5);
  137.       } else {
  138.         R_value = y_val;
  139.         ledcWrite(1, R_value);
  140.       }
  141.       Serial.print(" z_button , ") ;
  142.       Serial.print(z_button) ;
  143.       Serial.print(" c_button , ") ;
  144.       Serial.print(c_button) ;
  145.       Serial.print(" x_val , ") ;
  146.       Serial.print(x_val) ;
  147.       Serial.print(" y_val , ") ;
  148.       Serial.print(y_val) ;
  149.       Serial.print(" L_value , ") ;
  150.       Serial.print(L_value) ;
  151.       Serial.print(" R_value , ") ;
  152.       Serial.println(R_value) ;
  153.     }
  154.     digitalWrite(Connect_LED, HIGH);
  155.   } else {
  156.     //Serial.println("Not connected");
  157.     doConnect = true;
  158.     digitalWrite(Connect_LED, LOW);
  159.     y_val = 0;
  160.   }
  161. }