//ArduinoIDEにコピーして使います。　by Paradise
  1. #include <Wire.h>
  2. #include <BLEDevice.h>
  3. #include <BLEServer.h>
  4. #include <BLEUtils.h>
  5. #include <BLE2902.h>
  7. #define JOY_OFFSET_X 127 // 原点（極0を求めるオフセット値
  8. #define JOY_OFFSET_Y 127 // 原点（極0を求めるオフセット値
  9. #define I2C_SDA 8 // Nuncuck SDA input ESP32=21 C3=8
  10. #define I2C_CLK 9 // Nuncuck CLK input ESP32=22 C3=9
  11. BLECharacteristic *pCharacteristic;
  12. bool deviceConnected = false;
  14. uint8_t nunbuff[12]; // array to store ESP output
  15. int cnt = 0;
  16. int x_val = 0; // 1byte Data
  17. int y_val = 0; // 1byte Data
  18. int z_val = 0; // 1byte Data
  19. char number, speed;
  20. float r, t;
  21. int r_val;
  22. //Online UUID Generator https://www.uuidgenerator.net/     ＊ここでUUIDを取得します。
  23. #define SERVICE_UUID "********-4458-11e9-b210-************"
  24. #define CHARACTERISTIC_UUID "********-4458-11e9-b210-************"
  25. #define DEVICE_NAME "ESP32_BLE"
  27. class MyServerCallbacks : public BLEServerCallbacks {
  28.   void onConnect(BLEServer *pServer) {
  29.     deviceConnected = true;
  30.   };
  32.   void onDisconnect(BLEServer *pServer) {
  33.     deviceConnected = false;
  34.   }
  35. };
  37. void setup() {
  38.   Serial.begin(115200);
  39.   Wire.begin(); // join i2c bus with address 0x52
  40.   nunchuck_init(); // send the initilization handshake
  41.   BLEDevice::init(DEVICE_NAME);
  42.   BLEServer *pServer = BLEDevice::createServer();
  43.   pServer->setCallbacks(new MyServerCallbacks());
  44.   BLEService *pService = pServer->createService(SERVICE_UUID);
  45.   pCharacteristic = pService->createCharacteristic(
  46.     CHARACTERISTIC_UUID,
  47.     BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_INDICATE);
  49.   pCharacteristic->addDescriptor(new BLE2902());
  50.   pService->start();
  51.   pServer->getAdvertising()->start();
  52.   Serial.println("Waiting a client connection to notify...");
  53. }
  55. void nunchuck_init() {
  56.   Wire.beginTransmission(0x52); // transmit to device 0x52
  57.   Wire.write(0x40); // sends memory address *(blackNunchuck:0xF0）
  58.   Wire.write(0x00); // sends sent a zero. *(blackNunchuck:0x55）
  59.   Wire.endTransmission(); // stop transmitting
  60. }
  62. void send_zero() {
  63.   Wire.beginTransmission(0x52); // transmit to device 0x52
  64.   Wire.write(0x00); // sends one byte
  65.   Wire.endTransmission(); // stop transmitting
  66. }
  68. void loop() {
  69.   Wire.requestFrom(0x52, 6); // request data from nunchuck
  70.   while (Wire.available()) {
  71.     nunbuff[cnt] = nunchuk_decode_byte(Wire.read());
  72.     cnt++;
  73.   }
  74.   int z = 0;
  75.   int c = 0;
  77.   if (cnt >= 5) {
  78.     x_val = nunbuff[0];
  79.     y_val = nunbuff[1];
  80.     x_val = x_val + 5; // 最初に誤差補正をします。
  81.     y_val = y_val - 7; // 最初に誤差補正をします。
  82.     //Serial.print(" x_val = ");
  83.     //Serial.print(x_val);
  84.     //Serial.print(" y_val = ");
  85.     //Serial.print(y_val);
  86.     //Serial.print(" z_val = ");
  87.     //Serial.println(z_val);
  89.     if ((nunbuff[5] >> 0) & 1) z = 1; // The first bit of data of the 5byte (Nuncuck Z-button)
  90.     if ((nunbuff[5] >> 1) & 1) c = 1; // The second bit of data of the 5byte (Nuncuck C-button)
  91.     if ((z == 1) & (c == 1)) z_val = 0;
  92.     if ((z == 0) & (c == 1)) z_val = 1;
  93.     if ((z == 1) & (c == 0)) z_val = 2;
  94.     if ((z == 0) & (c == 0)) z_val = 3;
  95.     delay(1);
  96.   }
  97.   cnt = 0;
  98.   send_zero(); // send the request for next bytes
  99.   delay(10);
  101.   x_val = x_val - JOY_OFFSET_X; // 中心点を0にオフセット
  102.   y_val = JOY_OFFSET_Y - y_val; // 中心点を0にオフセット
  104.   r = sqrt((float)x_val * x_val + (float)y_val * y_val);
  105.   int r_val = r;
  106.   if (r_val <= 50) { // MotorSpeed変更用 PWM値呼び出しキャラクターの選択
  107.     speed = 'A';
  108.   } else if ((r_val >= 51) & (r_val <= 60)) {
  109.     speed = 'B';
  110.   } else if ((r_val >= 61) & (r_val <= 70)) {
  111.     speed = 'C';
  112.   } else if ((r_val >= 71) & (r_val <= 80)) {
  113.     speed = 'D';
  114.   } else if ((r_val >= 81) & (r_val <= 90)) {
  115.     speed = 'E';
  116.   } else if (r_val >= 91) {
  117.     speed = 'F';
  118.   }
  120.   if (x_val == 0) { // 極座標Pの位置により、角度Tを求めます。
  121.     if (y_val > 0) {
  122.       t = 90;
  123.     } else if (y_val < 0) {
  124.       t = 270;
  125.     } else {
  126.       t = 0;
  127.     }
  128.   } else {
  129.     t = atan((float)y_val / x_val) / 3.141592 * 180.0;
  130.     if (x_val < 0) {
  131.       t += 180;
  132.     } else if (y_val < 0) {
  133.       t += 360;
  134.     }
  135.   }
  137.   if (r > 50) { // ジョイスチックの位置が半径（ｒ）が50を超えると計算開始
  138.     if (t < 22) {
  139.       if (z_val == 1) { //Ｚボタンが押された場合
  140.         number = '#';
  141.         //MoveRight();　右横移動
  142.         //Serial.println("#");
  143.       } else {
  144.         number = '6';
  145.         //TurnRight();　右旋回
  146.         //Serial.println("6");
  147.       }
  148.     } else if (t < 67) {
  149.       number = '9';
  150.       //MoveRearRight();　右斜め後方移動
  151.       //Serial.println("9");
  152.     } else if (t < 112) {
  153.       number = '8';
  154.       //MoveBackward();　後進
  155.       //Serial.println("8");
  156.     } else if (t < 157) {
  157.       number = '7';
  158.       //MoveRearLeft();　左斜め後方移動
  159.       //Serial.println("7");
  160.     } else if (t < 202) {
  161.       if (z_val == 1) { //Ｚボタンが押された場合
  162.         number = '*';
  163.         //MoveLeft();　左横移動
  164.         //Serial.println("*");
  165.       } else {
  166.         number = '4';
  167.         //TurnLeft();　左旋回
  168.         //Serial.println("4");
  169.       }
  170.     } else if (t < 247) {
  171.       number = '1';
  172.       //MoveFrontLeft();　左斜め前方移動
  173.       //Serial.println("1");
  174.     } else if (t < 292) {
  175.       number = '2';
  176.       //MoveForward();　前進
  177.       //Serial.println("2");
  178.     } else if (t < 337) {
  179.       number = '3';
  180.       //MoveFrontRight();　右斜め前方移動
  181.       //Serial.println("3");
  182.     } else {
  183.       if (z_val == 1) { //Ｚボタンが押された場合
  184.         number = '#';
  185.         //MoveRight();　右横移動
  186.         //Serial.println("#");
  187.       } else {
  188.         number = '6';
  189.         //TurnRight();　右旋回
  190.         //Serial.println("6");
  191.       }
  192.     }
  193.   } else {
  194.     number = 'S';
  195.     //Stop();　停止
  196.     //Serial.println("S");
  197.   }
  198.   if (deviceConnected) { // コネクトした場合、numberとspeedの状態を通知する
  199.     char str1 = number;
  200.     char str2 = speed;
  201.     char str3[10];
  202.     sprintf(str3, "%d,%d\n", str1, str2);
  203.     printf("%s", str3);
  204.     //Serial.println(str3);
  205.     pCharacteristic->setValue(str3);
  206.     pCharacteristic->notify();
  207.   }
  208. }
  210. char nunchuk_decode_byte(char x) {
  211.   x = (x ^ 0x17) + 0x17;
  212.   return x;
  213. }