●ESP32側のドラッグコントロール対応

 「mm13」を改造してドラッグコントロール対応にしていきます。主な変更箇所はBLE受信のところと、loop()内のサーボ駆動用の角度計算部分です。歩行用の配列8行の半分の4行分を、BLEで送られてくる100分割に割り当てて決め打ちでコード中に数値を書いてしまっています。
 BLE受信部でvalue.length() == 5としていますが、これはAndroidのプログラムで
byte[] command = new byte[5];
としたためです。実際に使っているのは3byteのみです。

#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
#define FWDBWD  7
#define RIGHTSTEP  8
#define RIGHTTURN  9
#define LEFTSTEP  10
#define LEFTTURN  11
#define ARM  12

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, -20,  0,  0,-30,  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, 90, 80,100,100, 90, 8}, //左重心
  {90, 90, 90, 80, 70, 90, 90, 70, 70, 90, 90, 90, 4},
  {90, 90, 90, 80, 80, 90, 90, 90, 80, 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の割り込みを知らせる
uint8_t dragLR;
uint8_t dragFrame;
uint8_t manuHandRX;
uint8_t manuHandRY;
uint8_t manuHandLX;
uint8_t manuHandLY;
 
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() == 5) {
        byte bData = (uint8_t)value[0];
        switch(bData){
          case 127:
            bData = (uint8_t)value[1];
            dragLR = bData;
            bData = (uint8_t)value[2];
            dragFrame = bData;
            if(actionMode != FWDBWD){
              actionMode = FWDBWD;
              memcpy(motionAngles, fwrdAngles, sizeof(fwrdAngles));
              maxRows = sizeof(fwrdAngles) / sizeof(*fwrdAngles) - 1;
            }
            break;
          case 126:
            bData = (uint8_t)value[1];
            dragLR = bData;
            bData = (uint8_t)value[2];
            dragFrame = bData;
            if(actionMode != RIGHTSTEP){
              actionMode = RIGHTSTEP;
              memcpy(motionAngles, rghtAngles, sizeof(rghtAngles));
              maxRows = sizeof(rghtAngles) / sizeof(*rghtAngles) - 1;
            }
            break;
          case 125:
            bData = (uint8_t)value[1];
            dragLR = bData;
            bData = (uint8_t)value[2];
            dragFrame = bData;
            if(actionMode != RIGHTTURN){
              actionMode = RIGHTTURN;
              memcpy(motionAngles, rtrnAngles, sizeof(rtrnAngles));
              maxRows = sizeof(rtrnAngles) / sizeof(*rtrnAngles) - 1;
            }
            break;
          case 124:
            bData = (uint8_t)value[1];
            dragLR = bData;
            bData = (uint8_t)value[2];
            dragFrame = bData;
            if(actionMode != LEFTSTEP){
              actionMode = LEFTSTEP;
              memcpy(motionAngles, leftAngles, sizeof(leftAngles));
              maxRows = sizeof(leftAngles) / sizeof(*leftAngles) - 1;
            }
            break;
          case 123:
            bData = (uint8_t)value[1];
            dragLR = bData;
            bData = (uint8_t)value[2];
            dragFrame = bData;
            if(actionMode != LEFTTURN){
              actionMode = LEFTTURN;
              memcpy(motionAngles, ltrnAngles, sizeof(ltrnAngles));
              maxRows = sizeof(ltrnAngles) / sizeof(*ltrnAngles) - 1;
            }
            break;
          case 122:
          actionMode = ARM;
            bData = (uint8_t)value[1];
            manuHandRX = bData;
            bData = (uint8_t)value[2];
            manuHandRY = bData;
            bData = (uint8_t)value[3];
            manuHandLX = bData;
            bData = (uint8_t)value[4];
            manuHandLY = bData;
            break;
        }
        
      }else if(value.length() == 1) {
      //if (value.length() > 0) {
        //for (int i = 0; i < value.length(); i++){
          byte bData = (uint8_t)value[0];
          Serial.print(bData);
          if(bData == 0 || bData == 7 || bData == 9) {
            switch(bData){
            case 0:  //"0"ボタン
              start_playback();
              break;
            case 7:  //"7"ボタン  
              digitalWrite(14,HIGH);
              break;
            case 9:  //"9"ボタン  
              digitalWrite(14,LOW);
              break;
            default:
              break;
            }
          } else {
            switch(bData){  //<-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(1, 80, true);
  timerAttachInterrupt(timer2, &onTimer2, true);
  timerAlarmWrite(timer2, 30000, true);
  timerAlarmEnable(timer2);
   
  // 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;
    if(actionMode == ARM){
      tempAngles[4] = 70-(manuHandLY-50)*1.5;
      tempAngles[5] = 90-(manuHandLX-50)*1.5;
      tempAngles[6] = 90+(manuHandRX-50)*1.5;
      tempAngles[7] = 110+(manuHandRY-50)*1.5;
    }else if(actionMode == FWDBWD
        || actionMode == RIGHTTURN
        || actionMode == LEFTTURN){
      if(dragLR == 1){
        if(dragFrame < 25){
          keyFrame = 0;
          divCounter = dragFrame;
        }else if(dragFrame < 50){
          keyFrame = 1;
          divCounter = dragFrame - 25;
        }else if(dragFrame < 75){
          keyFrame = 2;
          divCounter = dragFrame - 50;
        }else{
          keyFrame = 3;
          divCounter = dragFrame - 75;
        }
      }else{
        if(dragFrame < 25){
          keyFrame = 4;
          divCounter = dragFrame;
        }else if(dragFrame < 50){
          keyFrame = 5;
          divCounter = dragFrame - 25;
        }else if(dragFrame < 75){
          keyFrame = 6;
          divCounter = dragFrame - 50;
        }else{
          keyFrame = 7;
          divCounter = dragFrame - 75;
        }
      }
      nextKeyFrame = keyFrame + 1;
      if(nextKeyFrame > 7)nextKeyFrame = 0;
      for(int i=0; i<12; i++) {
        tempAngles[i] = motionAngles[keyFrame][i] +
            int8_t((motionAngles[nextKeyFrame][i] - motionAngles[keyFrame][i])
             * divCounter / 25);
      }
    }else if(actionMode == RIGHTSTEP
        || actionMode == LEFTSTEP){
      if(dragLR == 1){
        if(dragFrame < 33){
          keyFrame = 0;
          divCounter = dragFrame;
        }else if(dragFrame < 66){
          keyFrame = 1;
          divCounter = dragFrame - 33;
        }else{
          keyFrame = 2;
          divCounter = dragFrame - 66;
        }
      }else{
        if(dragFrame < 33){
          keyFrame = 3;
          divCounter = dragFrame;
        }else if(dragFrame < 66){
          keyFrame = 4;
          divCounter = dragFrame - 33;
        }else{
          keyFrame = 5;
          divCounter = dragFrame - 66;
        }
      }
      nextKeyFrame = keyFrame + 1;
      if(nextKeyFrame > 5)nextKeyFrame = 0;
      for(int i=0; i<12; i++) {
        tempAngles[i] = motionAngles[keyFrame][i] +
            int8_t((motionAngles[nextKeyFrame][i] - motionAngles[keyFrame][i])
             * divCounter / 33);
      }
    }else{
      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);
  }
}

<= マイクロマシーン チュートリアル(14)
<= マイクロマシーン チュートリアル (16)
チュートリアルベース