xbox_controller_ble_motor

//ESP32 Arduino Core 1.0.4
//NimBLE library 1.20

#define FASTLED_ESP32_I2S true
#include <FastLED.h>
#include <Arduino.h>
#include <NimBLEDevice.h>
#include <Wire.h>

#define NUM_LEDS 12
#define LED_PIN 27
#define BTN_PIN 39
CRGB leds[NUM_LEDS];

//コントローラーの値代入用
int lx = 0;
int ly = 0;
int rx = 0;
int ry = 0;
int lt = 0;
int rt = 0;

//モーター出力設定
int8_t speed_sendbuff[4] = { 0 };
//進行方向用各要素定義
int8_t MoveY[4] = { 50, 50, 50, 50 };
int8_t MoveX[4] = { 50, -50, -50, 50 };
int8_t ROTATE[4] = { 30, -30, 30, -30 };

//ポーリング用
unsigned long lastMotor = 0;
unsigned long lastupdate = 0;

void scanEndedCB(NimBLEScanResults results);

static NimBLEAdvertisedDevice* advDevice;

bool scanning = false;
bool connected = false;
static uint32_t scanTime = 0; /** 0 = scan forever */

static NimBLEAddress targetDeviceAddress("c8:3f:26:8b:5f:69");

static NimBLEUUID uuidServiceUnknown("00000001-5f60-4c4f-9c83-a7953298d40d");
static NimBLEUUID uuidServiceGeneral("1801");
static NimBLEUUID uuidServiceBattery("180f");
static NimBLEUUID uuidServiceHid("1812");
static NimBLEUUID uuidCharaReport("2a4d");
static NimBLEUUID uuidCharaPnp("2a50");
static NimBLEUUID uuidCharaHidInformation("2a4a");
static NimBLEUUID uuidCharaPeripheralAppearance("2a01");
static NimBLEUUID uuidCharaPeripheralControlParameters("2a04");

class ClientCallbacks : public NimBLEClientCallbacks {
  void onConnect(NimBLEClient* pClient) {
    Serial.println("Connected");
    connected = true;
    leds[0].setRGB(0,0,10);
    FastLED.show();
    // pClient->updateConnParams(120,120,0,60);
  };

  void onDisconnect(NimBLEClient* pClient) {
    Serial.print(pClient->getPeerAddress().toString().c_str());
    Serial.println(" Disconnected");
    connected = false;
    leds[0].setRGB(10,0,0);
    FastLED.show();
  };

  /** Called when the peripheral requests a change to the connection parameters.
   *  Return true to accept and apply them or false to reject and keep
   *  the currently used parameters. Default will return true.
   */
  bool onConnParamsUpdateRequest(NimBLEClient* pClient,
                                 const ble_gap_upd_params* params) {
    Serial.print("onConnParamsUpdateRequest");
    if (params->itvl_min < 24) { /** 1.25ms units */
      return false;
    } else if (params->itvl_max > 40) { /** 1.25ms units */
      return false;
    } else if (params->latency > 2) { /** Number of intervals allowed to skip */
      return false;
    } else if (params->supervision_timeout > 100) { /** 10ms units */
      return false;
    }

    return true;
  };

  /********************* Security handled here **********************
  ****** Note: these are the same return values as defaults ********/
  uint32_t onPassKeyRequest() {
    Serial.println("Client Passkey Request");
    /** return the passkey to send to the server */
    return 0;
  };

  bool onConfirmPIN(uint32_t pass_key) {
    Serial.print("The passkey YES/NO number: ");
    Serial.println(pass_key);
    /** Return false if passkeys don't match. */
    return true;
  };

  /** Pairing process complete, we can check the results in ble_gap_conn_desc */
  void onAuthenticationComplete(ble_gap_conn_desc* desc) {
    Serial.println("onAuthenticationComplete");
    if (!desc->sec_state.encrypted) {
      Serial.println("Encrypt connection failed - disconnecting");
      /** Find the client with the connection handle provided in desc */
      NimBLEDevice::getClientByID(desc->conn_handle)->disconnect();
      return;
    }
  };
};

/** Define a class to handle the callbacks when advertisments are received */
class AdvertisedDeviceCallbacks : public NimBLEAdvertisedDeviceCallbacks {
  void onResult(NimBLEAdvertisedDevice* advertisedDevice) {
    Serial.print("Advertised Device found: ");
    Serial.println(advertisedDevice->toString().c_str());
    Serial.printf("name:%s, address:%s\n", advertisedDevice->getName().c_str(),
                  advertisedDevice->getAddress().toString().c_str());
    Serial.printf("uuidService:%s\n",
                  advertisedDevice->haveServiceUUID()
                      ? advertisedDevice->getServiceUUID().toString().c_str()
                      : "none");

    if (advertisedDevice->getAddress().equals(targetDeviceAddress))
    // if (advertisedDevice->isAdvertisingService(uuidServiceHid))
    {
      Serial.println("Found Our Service");
      /** stop scan before connecting */
      NimBLEDevice::getScan()->stop();
      /** Save the device reference in a global for the client to use*/
      advDevice = advertisedDevice;
    }
  };
};

unsigned long printInterval = 100UL;

/** Notification / Indication receiving handler callback */
void notifyCB(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData,
              size_t length, bool isNotify) {
  static bool isPrinting = false;
  static unsigned long printedAt = 0;
  if (isPrinting || millis() - printedAt < printInterval) return;
  isPrinting = true;
  std::string str = (isNotify == true) ? "Notification" : "Indication";
  str += " from ";
  /** NimBLEAddress and NimBLEUUID have std::string operators */
  str += std::string(
      pRemoteCharacteristic->getRemoteService()->getClient()->getPeerAddress());
  str += ": Service = " +
         std::string(pRemoteCharacteristic->getRemoteService()->getUUID());
  str += ", Characteristic = " + std::string(pRemoteCharacteristic->getUUID());
  // str += ", Value = " + std::string((char*)pData, length);
  Serial.println(str.c_str());
  //lx = pData[1] << 8 | pData[0];
  //ly = pData[3] << 8 | pData[2];
  //ry = pData[5] << 8 | pData[4];
  //rx = pData[7] << 8 | pData[6];
  //lt = pData[9] << 8 | pData[8];
  //rt = pData[11] << 8 | pData[10];
  //Serial.print("info: ");
  //Serial.print(lx);
  //Serial.println("");
  //Serial.print(pData[1]);
  //Serial.println("");
  //モータコントローラが8bitなので下位だけ使用
  lx = pData[1] - 127;
  ly = pData[3] - 127;
  ry = pData[5] - 127;
  rx = pData[7] - 127;
  lt = pData[9];
  rt = pData[11];


  /*Serial.print("value: ");
  for (int i = 0; i < length; ++i) {
    Serial.printf(" %02x", pData[i]);
  }
  Serial.println("");*/
  printedAt = millis();
  isPrinting = false;
}

void scanEndedCB(NimBLEScanResults results) {
  Serial.println("Scan Ended");
  scanning = false;
}

static ClientCallbacks clientCB;

void charaPrintId(NimBLERemoteCharacteristic* pChara) {
  Serial.printf("s:%s c:%s h:%d",
                pChara->getRemoteService()->getUUID().toString().c_str(),
                pChara->getUUID().toString().c_str(), pChara->getHandle());
}

void printValue(std::__cxx11::string str) {
  Serial.printf("str: %s\n", str.c_str());
  Serial.printf("hex:");
  for (auto v : str) {
    Serial.printf(" %02x", v);
  }
  Serial.println("");
}

void charaRead(NimBLERemoteCharacteristic* pChara) {
  if (pChara->canRead()) {
    charaPrintId(pChara);
    Serial.println(" canRead");
    auto str = pChara->readValue();
    if (str.size() == 0) {
      str = pChara->readValue();
    }
    printValue(str);
  }
}

void charaSubscribeNotification(NimBLERemoteCharacteristic* pChara) {
  if (pChara->canNotify()) {
    charaPrintId(pChara);
    Serial.println(" canNotify ");
    if (pChara->subscribe(true, notifyCB, true)) {
      Serial.println("set notifyCb");
      // return true;
    } else {
      Serial.println("failed to subscribe");
    }
  }
}

bool afterConnect(NimBLEClient* pClient) {
  for (auto pService : *pClient->getServices(true)) {
    auto sUuid = pService->getUUID();
    if (!sUuid.equals(uuidServiceHid)) {
      continue;  // skip
    }
    Serial.println(pService->toString().c_str());
    for (auto pChara : *pService->getCharacteristics(true)) {
      charaRead(pChara);
      charaSubscribeNotification(pChara);
    }
  }

  return true;
}

/** Handles the provisioning of clients and connects / interfaces with the
 * server */
bool connectToServer(NimBLEAdvertisedDevice* advDevice) {
  NimBLEClient* pClient = nullptr;

  /** Check if we have a client we should reuse first **/
  if (NimBLEDevice::getClientListSize()) {
    pClient = NimBLEDevice::getClientByPeerAddress(advDevice->getAddress());
    if (pClient) {
      pClient->connect();
    }
  }

  /** No client to reuse? Create a new one. */
  if (!pClient) {
    if (NimBLEDevice::getClientListSize() >= NIMBLE_MAX_CONNECTIONS) {
      Serial.println("Max clients reached - no more connections available");
      return false;
    }

    pClient = NimBLEDevice::createClient();

    Serial.println("New client created");

    pClient->setClientCallbacks(&clientCB, false);
    pClient->setConnectionParams(12, 12, 0, 51);
    pClient->setConnectTimeout(5);
    pClient->connect(advDevice, false);
  }

  int retryCount = 5;
  while (!pClient->isConnected()) {
    if (retryCount <= 0) {
      return false;
    } else {
      Serial.println("try connection again " + String(millis()));
      delay(1000);
    }

    NimBLEDevice::getScan()->stop();
    pClient->disconnect();
    delay(500);
    // Serial.println(pClient->toString().c_str());
    pClient->connect(true);
    --retryCount;
  }

  Serial.print("Connected to: ");
  Serial.println(pClient->getPeerAddress().toString().c_str());
  Serial.print("RSSI: ");
  Serial.println(pClient->getRssi());

  pClient->discoverAttributes();

  bool result = afterConnect(pClient);
  if (!result) {
    return result;
  }

  Serial.println("Done with this device!");
  return true;
}

void setup() {
  FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LEDS);
  leds[0].setRGB(20,0,0);
  FastLED.show();
  Serial.begin(115200);
  Serial.println("Starting NimBLE Client");
  /** Initialize NimBLE, no device name spcified as we are not advertising */
  NimBLEDevice::init("");
  NimBLEDevice::setOwnAddrType(BLE_OWN_ADDR_RANDOM);
  NimBLEDevice::setSecurityAuth(true, true, true);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9); /** +9db */

  Wire.begin(26,32);//SDA SCL

}

void startScan() {
  scanning = true;
  auto pScan = NimBLEDevice::getScan();
  pScan->setAdvertisedDeviceCallbacks(new AdvertisedDeviceCallbacks());
  pScan->setInterval(45);
  pScan->setWindow(15);
  Serial.println("Start scan");
  pScan->start(scanTime, scanEndedCB);
}

void loop() {
  if (lastupdate + 2000 <= millis()) {
    lastupdate = millis();//次の実行のために時間更新
  if (!connected) {
    if (!scanning && advDevice == nullptr) {
      startScan();
    }
    if (advDevice != nullptr) {
      if (connectToServer(advDevice)) {
        Serial.println("Success! we should now be getting notifications");
      } else {
        Serial.println("Failed to connect");
      }
      advDevice = nullptr;
    }
  }
  }
  if (lastMotor + 50 <= millis()) {
    lastMotor = millis();//次の実行のために時間更新
    setspeed(lx/127.00,ly/-127.00,ry/127.00);
  }
  // Serial.println("scanning:" + String(scanning) + " connected:" + String(connected) + " advDevice is nullptr:" + String(advDevice == nullptr));
  //delay(2000);
}

uint8_t setspeed(float x, float y, float r) {  // 前後左右回転それぞれを係数と掛けて足す
    for (int i = 0; i < 4; i++) {
        //speed_sendbuff[i] = FORWARD[i] * f;
        //speed_sendbuff[i] += BACKWARD[i] * b;
        //speed_sendbuff[i] += RIGHT[i] * r;
        //speed_sendbuff[i] += LEFT[i] * l;
        //speed_sendbuff[i] += ROTATE_L[i] * rl;
        //speed_sendbuff[i] += ROTATE_R[i] * rr;
        speed_sendbuff[i] = MoveY[i] * y;
        speed_sendbuff[i] += MoveX[i] * x;
        speed_sendbuff[i] += ROTATE[i] * r;
    }
    float limit = 0.0;
    for (int i = 0; i < 4; i++) {
        // speedが100を超えないようにリミッターをかける
        limit = 100.0 / max(abs(speed_sendbuff[3]), max(abs(speed_sendbuff[2]), max(abs(speed_sendbuff[1]), abs(speed_sendbuff[0]))));
    }
    //    printf("limit = %f\n", limit);
    if (limit < 1.0) {
        for (int i = 0; i < 4; i++) {
            speed_sendbuff[i] = speed_sendbuff[i] * limit;
        }
    }

    //Serial.print(speed_sendbuff[0]);
    //Serial.print(",");
    //Serial.print(speed_sendbuff[1]);
    //Serial.print(",");
    //Serial.print(speed_sendbuff[2]);
    //Serial.print(",");
    //Serial.println(speed_sendbuff[3]);

    return I2CWritebuff(0x00, (uint8_t*) speed_sendbuff, 4);
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++)
    {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}