PLEN:bit：M5Stackからのモーションコントロール

※ 以下で行っていることは、PLEN:bit の基本的な使い方とは外れていると思いますので、そのあたり理解した上でお読みください。

M5Stack との I2C 通信方法と、PLEN:blt のモーションデータフォーマットが分かったので、M5Stack でモーションデータを取得して、それを再生してみます。せっかくなので、モーション再生時に、AquesTalkを用いてしゃべらせてみました。

AquesTalk ESP32
https://www.a-quest.com/products/aquestalk_esp32.html

ソースはこちら。

#include <M5Stack.h>
const int LED_PIN = 2;

int servoSetInit[] = {1000, 630, 300, 600, 240, 600, 1000, 720};
int servoAngle[] =  {1000, 630, 300, 600, 240, 600, 1000, 720};
void setup(){
  M5.begin();
  Wire.begin();

  pinMode(LED_PIN,OUTPUT);
  digitalWrite( LED_PIN, HIGH );
  M5.Lcd.fillScreen(TFT_BLACK);
  M5.Lcd.setTextColor(TFT_WHITE);
  M5.Lcd.setTextSize(4);
  delay( 1000 );
  secretIncantation();
}

void loop(){
  M5.update();
  if (M5.BtnA.wasReleased()){
    delay(5000);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.println("Hello, World!");
    servoWrite(6,0);
    delay(1000);
    servoWrite(6,100);
    delay(500);
    M5.Lcd.println("");
    M5.Lcd.println("PLEN:M5Stack");

    String motionData = reep(0x32 + 4 * 860);
    playMotion(motionData);
  }
  if (M5.BtnC.wasReleased()){
    servoInitialSet();
  }
  delay(10);
}

void write8(int addr, int val){
  Wire.beginTransmission(0x6A);
  Wire.write(addr);
  Wire.write(val);
  Wire.endTransmission();
  //0: 成功
  //1: 送信バッファ溢れ
  //2: アドレス送信時にNACKを受信
  //3: データ送信時にNACKを受信
  //4: その他エラー
}

void secretIncantation() {
  Serial.println("secretIncantation");
    write8(0xFE, 0x85);//PRE_SCALE
    write8(0xFA, 0x00);//ALL_LED_ON_L
    write8(0xFB, 0x00);//ALL_LED_ON_H
    write8(0xFC, 0x66);//ALL_LED_OFF_L
    write8(0xFD, 0x00);//ALL_LED_OFF_H
    write8(0x00, 0x01);
    servoInitialSet();
}

void servoWrite(int num, int degrees) {
  //Serial.printf("ServoWrite:%d,%d\n",num,degrees);
  int servoNum = 0x08;
  bool highByte = false;
  int pwmVal = degrees * 100 * 226 / 10000;
  pwmVal = pwmVal + 0x66;
  if (pwmVal > 0xFF) {
    highByte = true;
  }
  write8(servoNum + num * 4, pwmVal);
  if (highByte) {
      write8(servoNum + num * 4 + 1, 0x01);
  } else {
      write8(servoNum + num * 4 + 1, 0x00);
  }
}

void servoInitialSet() {
  for(int i = 0;i < 8;i++){ 
    servoWrite(i, servoSetInit[i] / 10);
  }
}

String reep(int eepAdr) { //0xFFが出るまで読み、文字列として返す String result = ""; int romAdr1 = 0x56; boolean dataEnd = false; int readAddress = eepAdr; int readSize = 43; while(dataEnd == false){ Wire.beginTransmission(romAdr1); Wire.write(readAddress >> 8);
  Wire.write(readAddress & 0xFF);
  Wire.endTransmission();
  Wire.requestFrom(romAdr1, readSize);
  for(int index = 0;index < readSize;index++){
    if(Wire.available()){
      char c = Wire.read();      // 1バイトを受信
      if(c == 0xFF){
        dataEnd = true;
        break;
      }
      //Serial.print(c);
      result = result + String(c);
    }else{
      dataEnd = true;
      break;
    }
  }
    readAddress = readAddress + readSize;
  }
  return result;
}

void playMotion(String motionData){
  //Serial.println("PlayMotion");
  int offset = 0;
  while(offset < motionData.length()){ //>MF
    String checkData;
    checkData = motionData.substring(offset, offset + 3);
    if(checkData.equals(">MF")==false){
      Serial.printf("ERROR:OFFSET=%d\n",offset);
      return;
    }
    offset = offset + 3;
    offset = offset + 4;

    checkData = motionData.substring(offset, offset + 4);
    int msec = strtol(checkData.c_str() , NULL , 16);
    offset = offset + 4;
    int angle[8];
    //Serial.printf("SetAngel:%d:",msec);
    for(int servoNo = 0;servoNo < 8;servoNo++){ checkData = motionData.substring(offset, offset + 4); angle[servoNo] = strtol(checkData.c_str() , NULL , 16); if(angle[servoNo] > 0x7FFF){
        angle[servoNo] = angle[servoNo] - 0x10000;
      }
      //Serial.printf(":%d", angle[servoNo] );
      offset = offset + 4;
    }
    //Serial.println();
    setAngle(angle,msec);
  }
}
void setAngle(int angle[], int msec){
  int step[] = {0,0,0,0,0,0,0,0};
  int realMsec = msec / 20;
  
  for(int val = 0;val < 8;val++){
    int target = servoSetInit[val] - angle[val];
    if (target != servoAngle[val]) { // Target != Present
      step[val] = (target - servoAngle[val]) / (realMsec);
    }
  }
  //Serial.println();
  for (int i = 0; i <= realMsec; i++) {
    for (int val = 0; val < 8; val++) {
      servoAngle[val] += step[val];
      servoWrite(val, (servoAngle[val] / 10));
      //Serial.printf("Servo:%d:%d\n",val,servoAngle[val] );
    }
  } 
}

Aボタンを押すと、右手を上げ下げした後に、モーション番号4（おじぎ）を再生するようにしています。

サーボ制御の部分は「PLEN:bit：M5Stackと接続」と同じです。

・void write8(int addr, int val)：アドレスを指定して値を書き込む
・void secretIncantation()：サーボ初期化
・void servoWrite(int num, int degrees)：サーボ番号と角度を指定してサーボ動作

「PLEN:bit：モーションデータ」のTypeScript を移植する感じで、モーションデータ読み込み関数を作成。

・String reep(int eepAdr)：アドレスを指定してモーションデータを読みこみ。

モーションデータに従って、モーション再生する関数を作成。

・void playMotion(String motionData)：reepで読み込んだモーションデータを引数にもらい、その順にサーボ状態を設定。

playMotion の中で、各サーボモータの状態を設定しているのは setAngle 関数です。

・void setAngle(int angle[], int msec)：時間msecをかけて、各サーボを angle[] 状態にする。

msec は、現状だと秒指定というわけではないです。なんとなくの動作時間ぐらいに思ってもらえれば…。

せっかくなので、M5Stack を PLEN:bit と合体させました。

M5Stack FIRE（M5GO）のボトムに針金をネジ止め。ユニバーサル基板のホールを通して、ねじって抜けないように。
少々強引な方法ですが、PLEN:bit に M5Stack FIRE を搭載できました！ pic.twitter.com/ZbtwBXzwJY

— Nochi (@shikarunochi) 2019年6月1日

実行！

PLEN:bit内 のモーションデータを M5Stack で読み込んでモーション再生。
AquesTalkの音声合成と合わせてみました。
さすがに前側が重すぎて、お辞儀モーションでふんばりきれず倒れてしまった。 pic.twitter.com/QrijzWn9Q4

— Nochi (@shikarunochi) 2019年6月3日

うまくモーション再生できた！が、M5Stack は重すぎたか…！

M5Stick-C を使えば、軽くできそうですね。Grove 端子から、I2C でコントロールできました。

M5Stick-CのGrove 端子からI2CでPLEN:bit動かせました！
Wire.begin(32,33);です。
ピンソケット（0,26,36）側は、どの組み合わせで接続しても動作できませんでした。残念…。 pic.twitter.com/X3KGCIvLQ5

— Nochi (@shikarunochi) 2019年6月4日

HOMEボタンを押したら腕を上げ下げするようにしてます。

#include <M5StickC.h>
int servoSetInit[] = {1000, 630, 300, 600, 240, 600, 1000, 720};
int servoAngle[] = {1000, 630, 300, 600, 240, 600, 1000, 720};
void setup() {
  M5.begin();
  Wire.begin(32,33);
  //Wire.begin(36,0);//ピンソケット側はどの組み合わせでもダメだった
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(TFT_BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.println("PLEN:");
  M5.Lcd.println("M5Stick-C");
  pinMode(M5_BUTTON_HOME, INPUT);
  secretIncantation();
}

void loop() {
  if(digitalRead(M5_BUTTON_HOME) == LOW){
    M5.Lcd.println("HOME BUTTON");
    servoWrite(6,0);
    delay(1000);
    servoWrite(6,100);
    delay(500);
  }
}

void write8(int addr, int val){
  Wire.beginTransmission(0x6A);
  Wire.write(addr);
  Wire.write(val);
  Wire.endTransmission();
  //Serial.print("Write:");
  //0: 成功
  //1: 送信バッファ溢れ
  //2: アドレス送信時にNACKを受信
  //3: データ送信時にNACKを受信
  //4: その他エラー
}

void secretIncantation() {
  Serial.println("secretIncantation");
    write8(0xFE, 0x85);//PRE_SCALE
    write8(0xFA, 0x00);//ALL_LED_ON_L
    write8(0xFB, 0x00);//ALL_LED_ON_H
    write8(0xFC, 0x66);//ALL_LED_OFF_L
    write8(0xFD, 0x00);//ALL_LED_OFF_H
    write8(0x00, 0x01);
    servoInitialSet();
}

void servoWrite(int num, int degrees) {
  //Serial.printf("ServoWrite:%d,%d\n",num,degrees);
  int servoNum = 0x08;
  bool highByte = false;
  int pwmVal = degrees * 100 * 226 / 10000;
  pwmVal = pwmVal + 0x66;
  if (pwmVal > 0xFF) {
    highByte = true;
  }
  write8(servoNum + num * 4, pwmVal);
  if (highByte) {
      write8(servoNum + num * 4 + 1, 0x01);
  } else {
      write8(servoNum + num * 4 + 1, 0x00);
  }
}

void servoInitialSet() {
  for(int i = 0;i < 8;i++){
    servoWrite(i, servoSetInit[i] / 10);
  }
}

Grove側の接続が必要になるのであればこういう感じか。もしくは縦にするか。M5Stick-Cの電源をピンソケット経由で外部から供給したい場合は、接続スペース的に、縦にするしかなさそうだ。 pic.twitter.com/GzbSgD9LRx

— Nochi (@shikarunochi) 2019年6月4日