- #include "BLEDevice.h"
- int Connect_LED = 33;
- int x_val, y_val, yr_val, yl_val, z_val, z_button, c_button;
- int RL_val, L_value, R_value, x_val2, x_val3;
- int Cv_r = 15; // R-Motor Correction value 補正値
- int Cv_l = 0; // L-Motor Correction value 補正値
- char* data;
- #define R_motor 26
- #define L_motor 27
- #define SERVICE_UUID "cdb068be-4458-11e9-b210-d663bd873d93"
- #define CHARACTERISTIC_UUID "cdb06c10-4458-11e9-b210-d663bd873d93"
- #define SERVER_NAME "esp32_BLE"
- static BLEUUID serviceUUID(SERVICE_UUID);
- static BLEUUID charUUID(CHARACTERISTIC_UUID);
- static BLEAddress *pServerAddress;
- static boolean doConnect = false;
- static boolean connected = false;
- static BLERemoteCharacteristic* pRemoteCharacteristic;
- static void notifyCallback(
- BLERemoteCharacteristic* pBLERemoteCharacteristic,
- uint8_t* pData,
- size_t length,
- bool isNotify) {
- }
- bool connectToServer(BLEAddress pAddress) {
- //Serial.print("Forming a connection to ");
- //Serial.println(pAddress.toString().c_str());
- BLEClient* pClient = BLEDevice::createClient();
- pClient->connect(pAddress);
- BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
- //Serial.print(" - Connected to server :");
- //Serial.println(serviceUUID.toString().c_str());
- if (pRemoteService == nullptr) {
- //Serial.print("Failed to find our service UUID: ");
- //Serial.println(serviceUUID.toString().c_str());
- return false;
- }
- pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
- //Serial.print(" - Found our characteristic UUID: ");
- //Serial.println(charUUID.toString().c_str());
- if (pRemoteCharacteristic == nullptr) {
- //Serial.print("Failed to find our characteristic UUID: ");
- return false;
- }
- //Serial.println(" - Found our characteristic");
- pRemoteCharacteristic->registerForNotify(notifyCallback);
- }
- class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
- void onResult(BLEAdvertisedDevice advertisedDevice) {
- //Serial.print("BLE Advertised Device found: ");
- //Serial.println(advertisedDevice.toString().c_str());
- //Serial.println(advertisedDevice.getName().c_str());
- if (advertisedDevice.getName() == SERVER_NAME) {
- //Serial.println(advertisedDevice.getAddress().toString().c_str());
- advertisedDevice.getScan()->stop();
- pServerAddress = new BLEAddress(advertisedDevice.getAddress());
- doConnect = true;
- }
- }
- };
- void setup() {
- Serial.begin(115200);
- BLEDevice::init(SERVER_NAME);
- BLEScan* pBLEScan = BLEDevice::getScan();
- pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
- pBLEScan->setActiveScan(true);
- pBLEScan->start(2);
- pinMode(Connect_LED, OUTPUT); digitalWrite(Connect_LED, LOW);// BLE Status LED
- ledcSetup(0, 500, 8); // (0ch,500Hz,8bit resolution)
- ledcAttachPin(L_motor, 0); // 0ch
- ledcSetup(1, 500, 8); // (0ch,500Hz,8bit resolution)
- ledcAttachPin(R_motor, 1); // 1ch
- }
- void loop() {
- if (doConnect == true) {
- if (connectToServer(*pServerAddress)) {
- Serial.println("We are now connected to the BLE Server.");
- connected = true;
- } else {
- Serial.println("We have failed to connect to the server; there is nothin more we will do.");
- connected = false;
- }
- doConnect = false;
- }
- if (connected) {
- std::string value = pRemoteCharacteristic->readValue();
- String strVal = value.c_str();
- char strNum = strVal.toInt();
- data = new char[strVal.length() + 1];
- strcpy(data, value.c_str());
- if (sscanf(data, "%d,%d,%d", &x_val, &y_val, &z_val) == 3) {
- if (z_val == 0)z_button = 1;
- if (z_val == 1)z_button = 0;
- if (z_val == 0)c_button = 1;
- if (z_val == 2)c_button = 0;
- if (z_val == 3) {
- z_button = 0;
- c_button = 0;
- }
- //Serial.print(" x_val , ") ; //x_val実測値:min=22,max=207,center=123
- //Serial.print(x_val) ;
- //Serial.print(" y_val , ") ; //y_val実測値:min=31,max=212,center=131
- //Serial.println(y_val) ;
- if (x_val < 113) {
- x_val = map(x_val, 22, 113, 50, 1);
- //実測に合わせ、X軸の入力最小値を22に設定、x_valの設定範囲を50~1とします。
- RL_val = 1;
- }
- else if (x_val > 133) {
- x_val = map(x_val, 133, 207, 1, 50);
- //実測に合わせ、X軸の入力最大値を207に設定、x_valの設定範囲を1~50とします。
- RL_val = 2;
- } else {
- x_val = 0; // X軸center値123の±10=113~133を不感帯をx_val=0とします。
- RL_val = 0;
- }
- yr_val = map(y_val, 31, 212, 0, 255 - Cv_r); // 右モータ:補正値15
- yr_val = max(yr_val, 0);
- yr_val = min(yr_val, 255 - Cv_r);
- yl_val = map(y_val, 31, 212, 0, 255 - Cv_l); // 左モータ:補正値0
- yl_val = max(yl_val, 0);
- yl_val = min(yl_val, 255 - Cv_l);
- if (RL_val == 1) {
- L_value = yl_val - (yl_val / 100 * x_val);
- //x_valを0~50に調節すると、yl_valが0~50%に変化します。
- ledcWrite(0, L_value);
- delay(5);
- } else {
- L_value = yl_val;
- ledcWrite(0, L_value);
- }
- if (RL_val == 2) {
- R_value = yr_val - (yr_val / 100 * x_val);
- //x_valを0~50に調節すると、yr_valが0~50%に変化します。
- ledcWrite(1, R_value);
- delay(5);
- } else {
- R_value = y_val;
- ledcWrite(1, R_value);
- }
- Serial.print(" z_button , ") ;
- Serial.print(z_button) ;
- Serial.print(" c_button , ") ;
- Serial.print(c_button) ;
- Serial.print(" x_val , ") ;
- Serial.print(x_val) ;
- Serial.print(" y_val , ") ;
- Serial.print(y_val) ;
- Serial.print(" L_value , ") ;
- Serial.print(L_value) ;
- Serial.print(" R_value , ") ;
- Serial.println(R_value) ;
- }
- digitalWrite(Connect_LED, HIGH);
- } else {
- //Serial.println("Not connected");
- doConnect = true;
- digitalWrite(Connect_LED, LOW);
- y_val = 0;
- }
- }