●スマートフォンからBLEでコントロール

*Android


*iPhone



 ESP32にはWiFi/Bluetooth機能が搭載されていますので、スマートフォンからロボットをリモートコントロールできます。ここではBLE(Bluetooth Low Energy)を使います。まずは既存のアプリからロボットと通信しLEDをON/OFFしてみます。
予めスマートフォンにBLEアプリをインストールしておいてください。
・Android:BLE Scanner
・iPhone:BLE Scanner
 ロボット側のプログラムはESP32のサンプルプログラムを利用します。「ファイル」->「スケッチ例」->「ESP32 BLE Arduino」->「BLE_write」を開き、mm12として保存します。そのままでも動きますが、一応LEDのON/OFF制御プログラムを付け加えておきます。

/*
    Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleWrite.cpp
    Ported to Arduino ESP32 by Evandro Copercini
*/

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string value = pCharacteristic->getValue();

      if (value.length() > 0) {
        Serial.println("*********");
        Serial.print("New value: ");
        for (int i = 0; i < value.length(); i++) {
          Serial.print(value[i]);
          if(value[i]=='1') digitalWrite(14, HIGH);
          else digitalWrite(14, LOW);
        }
        Serial.println();
        Serial.println("*********");
      }
    }
};

void setup() {
  Serial.begin(115200);

  Serial.println("1- Download and install an BLE scanner app in your phone");
  Serial.println("2- Scan for BLE devices in the app");
  Serial.println("3- Connect to MyESP32");
  Serial.println("4- Go to CUSTOM CHARACTERISTIC in CUSTOM SERVICE and write something");
  Serial.println("5- See the magic =)");

  BLEDevice::init("MyESP32");
  BLEServer *pServer = BLEDevice::createServer();

  BLEService *pService = pServer->createService(SERVICE_UUID);

  BLECharacteristic *pCharacteristic = pService->createCharacteristic(
                                         CHARACTERISTIC_UUID,
                                         BLECharacteristic::PROPERTY_READ |
                                         BLECharacteristic::PROPERTY_WRITE
                                       );

  pCharacteristic->setCallbacks(new MyCallbacks());

  pCharacteristic->setValue("Hello World");
  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->start();

  pinMode(14, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(2000);
}

スマートフォンのアプリを立ち上げてロボットの電源を入れると(USBケーブルの接続のみでも可)、「MyESP32」という名前で出てきますので「CONNECT」ボタンを押して接続します。"4FAF...."で始まるサービス(プログラムの13行目参照)を選択して、"BEB5....."で始まるキャラクタリスティックで「Write」ボタンを押して数字を書き込みます。"1"を送信するとLEDが点灯、"0"などを送信するとLEDが消灯するはずです。WriteはTextとByte Arrayとありますが、Textのほうで送ります。
ここでスマートフォンから送っているのは数値データの0,1...ではなく、文字の"0","1"...です。

●練習問題

mm11のプログラムを改造してスマートフォン対応にしてください。一応対応する数字は、赤外リモコンのボタンと同じにしておいてください。mm12-2で保存してください。

#include "audiodata.h"
#include <Wire.h>

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>

// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"

// remote
const uint8_t interruptPin = 16;
boolean  rmReceived = 0;  //信号受信完了した
uint8_t  i;               //受信データの桁
uint8_t  rmState = 0;     //信号受信状況
uint8_t  dataCode;        //データコード(8bit)
uint8_t  invDataCode;     //反転データコード(8bit)
uint16_t customCode;      //カスタムコード(16bit)
uint32_t rmCode;          //コード全体(32bit)
volatile uint32_t prevMicros = 0; //時間計測用

// sound
int wavCounter;
hw_timer_t * timer = NULL;
volatile SemaphoreHandle_t timerSemaphore;

// another timer
hw_timer_t * timer2 = NULL;
volatile SemaphoreHandle_t timerSemaphore2;

// action
#define STOP  0
#define FWRD  1
#define BWRD  2
#define RTRN  3
#define LTRN  4
#define RGHT  5
#define LEFT  6
uint8_t actionMode = STOP;
int16_t tempAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t offset[12] = {10,  4, -7,  8, 0,  0,  0,-10,  2, -2,-12, -2};//自機に合わせて変更

int16_t stopAngles[12] = {90,90,90,90,90,90,90,90,90,90,90,90};
int16_t leftAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rghtAngles[6][13] = {
// LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time 
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 90, 8},
  { 90, 90, 90, 80, 90, 90, 90, 90, 80, 90, 90, 80, 8},
  {100, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90,100, 90, 90, 90, 90,100, 90, 90, 90, 8},
  { 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 90, 8}
};
int16_t rtrnAngles[8][13] = {
// 常に右重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90,100, 80, 90, 90, 80, 90,100,100, 90, 8}, //右重心
  {90, 90, 90,100, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90,100, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t ltrnAngles[8][13] = {
// 常に左重心にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80, 90,100, 90, 90,100,100, 90, 90, 90, 8}, //左重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t bwrdAngles[8][13] = { //後退
// 順番を逆にしました。
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}, //左足前
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8}
};
int16_t fwrdAngles[8][13] = {
//LHR LHP LAP LAR LSP LSR RSR RSP RAR RAP RHP RHR time
  {90,100,100, 90, 90, 90, 90, 90, 90,100,100, 90, 8},
  {90, 90, 90, 80, 80, 90, 90, 80, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 90, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 80, 90, 80, 80, 90, 4}, //右足前
  {90, 80, 80, 90, 90, 90, 90, 90, 90, 80, 80, 90, 8},
  {90, 80, 80,100,100, 90, 90,100,100, 90, 90, 90, 8}, //右重心
  {90, 90, 90, 90,110, 90, 90,110,100, 90, 90, 90, 4},
  {90,100,100, 90,100, 90, 90,100,100, 90, 90, 90, 4}  //左足前
};
int16_t motionAngles[8][13];
uint8_t maxRows;
uint8_t divCounter;
uint8_t keyFrame;
uint8_t nextKeyFrame;
uint8_t svFlag = 0; //30msecの割り込みを知らせる

void init_pca9685() {
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //reset device
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x10);   //sleep mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0xFE);
  Wire.write(0x65); //set prescaler to 60Hz
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0x80);   //back to prev mode
  Wire.endTransmission();
  
  Wire.beginTransmission(0x40);
  Wire.write(0x0);
  Wire.write(0xa0);   //enable auto-increment
  Wire.endTransmission();
}

void set_angle() {
  int angle;
  Wire.beginTransmission(0x40);
  Wire.write(6);
  for(int i=0;i<12;i++){
    angle = 400 + (tempAngles[i] + offset[i] - 90)*2.8;
    Wire.write(0x0);
    Wire.write(0x0>>8);
    Wire.write(angle);
    Wire.write(angle>>8);
  }
  Wire.endTransmission();
}

void IRAM_ATTR onTimer(){
  xSemaphoreGiveFromISR(timerSemaphore, NULL);
  if(wavCounter < sound_length) dacWrite(25,sound_data[wavCounter++]);
  else stop_playback();
}

void IRAM_ATTR onTimer2(){
  xSemaphoreGiveFromISR(timerSemaphore2, NULL);
  svFlag = 1;
}

void rmUpdate() //信号が変化した
{
  uint32_t width; //パルスの幅を計測
  if(rmState != 0){
    width = micros() - prevMicros;  //時間間隔を計算
    if(width > 10000)rmState = 0; //長すぎ
    prevMicros = micros();  //次の計算用
  }
  switch(rmState){
    case 0: //信号未達
    prevMicros = micros();  //現在時刻(microseconds)を記憶
    rmState = 1;  //最初のOFF->ON信号を検出した
    i = 0;
    return;
    case 1: //最初のON状態
      if((width > 9500) || (width < 8500)){ //リーダーコード(9ms)ではない
        rmState = 0;
      }else{
        rmState = 2;  //ON->OFFで9ms検出
      }
      break;
    case 2: //9ms検出した
      if((width > 5000) || (width < 4000)){ //リーダーコード(4.5ms)ではない
        rmState = 0;
      }else{
        rmState = 3;  //OFF->ONで4.5ms検出
      }
      break;
    case 3: //4.5ms検出した
      if((width > 700) || (width < 400)){
        rmState = 0;
      }else{
        rmState = 4;  //ON->OFFで0.56ms検出した
      }
      break;
    case 4: //0.56ms検出した
      if((width > 1800) || (width < 400)){  //OFF期間(2.25-0.56)msより長い or (1.125-0.56)msより短い
        rmState = 0;
      }else{
        if(width > 1000){ //OFF期間長い -> 1
          bitSet(rmCode, (i));
        }else{             //OFF期間短い -> 0
          bitClear(rmCode, (i));
        }
        i++;  //次のbit
        if(i > 31){ //完了
          rmReceived = 1;
          return;
        }
        rmState = 3;  //次のON->OFFを待つ
      }
      break;
    }
}

void start_playback() {
  wavCounter = 0;
  timerAlarmEnable(timer);
  digitalWrite(2, LOW);
}

void stop_playback() {
  timerAlarmDisable(timer);
  digitalWrite(2, HIGH);
}

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string value = pCharacteristic->getValue();

      if (value.length() > 0) {
        for (int i = 0; i < value.length(); i++){
          Serial.print(value[i]);
          //if(value[i] == '1') digitalWrite(14, HIGH);
          //else digitalWrite(14, LOW);
          if(value[i] == '0' || value[i] == '7' || value[i] == '9') {
            switch(value[i]){
            case '0':  //"0"ボタン
              start_playback();
              break;
            case '7':  //"7"ボタン  
              digitalWrite(14,HIGH);
              break;
            case '9':  //"9"ボタン  
              digitalWrite(14,LOW);
              break;
            default:
              break;
            }
          } else {
            switch(value[i]){  //<-switch文を追加
            case '2':  //"2"ボタン
              actionMode = FWRD;
              memcpy(motionAngles, fwrdAngles, sizeof(fwrdAngles));
              maxRows = sizeof(fwrdAngles) / sizeof(*fwrdAngles) - 1;
              break;
            case '5':  //"5"ボタン
              actionMode = STOP;
              for(int i=0; i<12; i++) {
                tempAngles[i] = stopAngles[i];
              }
              set_angle();
              break;
            case '8':  //"8"ボタン
              actionMode = BWRD;
              memcpy(motionAngles, bwrdAngles, sizeof(bwrdAngles));
              maxRows = sizeof(bwrdAngles) / sizeof(*bwrdAngles) - 1;
              break;
            case '1':  //"1"ボタン
              actionMode = LTRN;
              memcpy(motionAngles, ltrnAngles, sizeof(ltrnAngles));
              maxRows = sizeof(ltrnAngles) / sizeof(*ltrnAngles) - 1;
              break;
            case '3':  //"3"ボタン
              actionMode = RTRN;
              memcpy(motionAngles, rtrnAngles, sizeof(rtrnAngles));
              maxRows = sizeof(rtrnAngles) / sizeof(*rtrnAngles) - 1;
              break;
            case '4':   //"4"ボタン
              actionMode = LEFT;
              memcpy(motionAngles, leftAngles, sizeof(leftAngles));
              maxRows = sizeof(leftAngles) / sizeof(*leftAngles) - 1;
              break;
            case '6':  //"6"ボタン
              actionMode = RGHT;
              memcpy(motionAngles, rghtAngles, sizeof(rghtAngles));
              maxRows = sizeof(rghtAngles) / sizeof(*rghtAngles) - 1;
              break;
            default:
            break;
            }
            divCounter = 0;
            keyFrame = 0;
            nextKeyFrame = 1;
          }
        }  
      }
    }
};

void setup() {
  //Serial.begin(115200);
  // audio
  timerSemaphore = xSemaphoreCreateBinary();
  timer = timerBegin(0, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 124, true);
  
  wavCounter = 0;
  pinMode(2, OUTPUT);
  digitalWrite(2, HIGH);

  attachInterrupt(digitalPinToInterrupt(interruptPin), rmUpdate, CHANGE);

  // led
  pinMode(14, OUTPUT);
  digitalWrite(14,LOW);

  // servo
  Wire.begin(21,22);
  Wire.setClock(100000);

  init_pca9685();
  set_angle();
  
  pinMode(19, OUTPUT);
  digitalWrite(19,LOW);

  timerSemaphore2 = xSemaphoreCreateBinary();
  timer2 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer2, &onTimer2, true);
  timerAlarmWrite(timer2, 30000, true);
  timerAlarmEnable(timer);
  
  // ble
  BLEDevice::init("MyESP32");
  BLEServer *pServer = BLEDevice::createServer();

  BLEService *pService = pServer->createService(SERVICE_UUID);

  BLECharacteristic *pCharacteristic = pService->createCharacteristic(
                                         CHARACTERISTIC_UUID,
                                         BLECharacteristic::PROPERTY_READ |
                                         BLECharacteristic::PROPERTY_WRITE
                                       );

  pCharacteristic->setCallbacks(new MyCallbacks());

  pCharacteristic->setValue("Hello World");
  pService->start();

  BLEAdvertising *pAdvertising = pServer->getAdvertising();
  pAdvertising->start();
}

void loop() {
  if(rmReceived){ //リモコン受信した
    detachInterrupt(digitalPinToInterrupt(interruptPin));
    rmReceived = 0;   //初期化
    rmState = 0;      //初期化
    //図とは左右が逆であることに注意
    customCode = rmCode;    //下16bitがcustomCode
    dataCode = rmCode >> 16;  //下16bitを捨てたあとの下8bitがdataCode
    invDataCode = rmCode >> 24; //下24bitを捨てたあとの下8bitがinvDataCode
    if((dataCode + invDataCode) == 0xff){   //反転確認
      if(dataCode == 22 || dataCode == 66 || dataCode == 74) {
        switch(dataCode){
        case 22:  //"0"ボタン
          start_playback();
          break;
        case 66:  //"7"ボタン  
          digitalWrite(14,HIGH);
          break;
        case 74:  //"9"ボタン  
          digitalWrite(14,LOW);
          break;
        default:
          break;
        }
      } else {
        switch(dataCode){  //<-switch文を追加
        case 24:  //"2"ボタン
          actionMode = FWRD;
          memcpy(motionAngles, fwrdAngles, sizeof(fwrdAngles));
          maxRows = sizeof(fwrdAngles) / sizeof(*fwrdAngles) - 1;
          break;
        case 28:  //"5"ボタン
          actionMode = STOP;
          for(int i=0; i<12; i++) {
            tempAngles[i] = stopAngles[i];
          }
          set_angle();
          break;
        case 82:  //"8"ボタン
          actionMode = BWRD;
          memcpy(motionAngles, bwrdAngles, sizeof(bwrdAngles));
          maxRows = sizeof(bwrdAngles) / sizeof(*bwrdAngles) - 1;
          break;
        case 12:  //"1"ボタン
          actionMode = LTRN;
          memcpy(motionAngles, ltrnAngles, sizeof(ltrnAngles));
          maxRows = sizeof(ltrnAngles) / sizeof(*ltrnAngles) - 1;
          break;
        case 94:  //"3"ボタン
          actionMode = RTRN;
          memcpy(motionAngles, rtrnAngles, sizeof(rtrnAngles));
          maxRows = sizeof(rtrnAngles) / sizeof(*rtrnAngles) - 1;
          break;
        case 8:   //"4"ボタン
          actionMode = LEFT;
          memcpy(motionAngles, leftAngles, sizeof(leftAngles));
          maxRows = sizeof(leftAngles) / sizeof(*leftAngles) - 1;
          break;
        case 90:  //"6"ボタン
          actionMode = RGHT;
          memcpy(motionAngles, rghtAngles, sizeof(rghtAngles));
          maxRows = sizeof(rghtAngles) / sizeof(*rghtAngles) - 1;
          break;
        default:
        break;
        }
        divCounter = 0;
        keyFrame = 0;
        nextKeyFrame = 1;
      }
    }
    attachInterrupt(digitalPinToInterrupt(interruptPin), rmUpdate, CHANGE);
  }
  if(actionMode != STOP && svFlag){
    svFlag = 0;
    divCounter++;
    if(divCounter >= motionAngles[nextKeyFrame][12]) {
      divCounter = 0;
      keyFrame = nextKeyFrame;
      nextKeyFrame++;
      if(nextKeyFrame > maxRows) nextKeyFrame = 0;
    }
    for(int i=0; i<12; i++) {
      tempAngles[i] = motionAngles[keyFrame][i] +
            int8_t((motionAngles[nextKeyFrame][i] - motionAngles[keyFrame][i])
             * divCounter / motionAngles[nextKeyFrame][12]);
    }
    set_angle();
    //delay(30);
  }
}

スマートフォンの入力に時間がかかりがちですので、ロボットが動いてテーブルから落下したりしないようにご注意ください。

<= マイクロマシーン チュートリアル(11)
チュートリアルベース
マイクロマシーン チュートリアル (13) =>