M5StackとProcessingでOrphe Trackをマネしてみた

はじめに

バーチャルマラソン大会に向けてモチベーションを上げるために活動量計を使用して参加することにしました。
自分の走りのデータを取得して見える化してくれます。
BLEでデバイスとスマートフォンを接続してデータを取得するタイプです。

使用するアプリ

• Orphe Track (Orphe Coreデバイスを使用するのに必要)
• Runmetrix (asics x CASIO MOTION SENSORデバイスを使用するのに必要)
• TATTA (バーチャルマラソン大会への参加に必要)

使用するデバイス

Orphe Core 1.1

データ取得の開始時と終了時にスマートフォンのアプリと接続して操作する必要があります。

SPBTLE-RFTR (BLEモジュールが使用されていることを確認)

MPU-9250 (仕様には6軸モーションセンサーと記載されているが、実際には9軸モーションセンサーが使用されていることを確認)

CMT-S20R-AS (asics x CASIO MOTION SENSOR)

GPS機能が搭載されているためデバイス単体でもデータ取得が可能ですが、GPS測位のアシストデータを更新するのにスマートフォンのアプリと接続して操作する必要があります。

実装

CMT-S20R-ASは腰の位置に付けるタイプで縦向きに使用するのが基本ですが、Orphe Coreは両足に(片足ずつ)デバイスを付けるタイプで足の動きに合わせて激しく動くので、面白いデータが取得できそうです。

M5StickC、M5Atom MatrixにはMPU-6886 (6軸モーションセンサー)が搭載されているため、Bluetooth SPP(Serial Port Profile)を使用してPC(macOS)のProcessingで見える化してみました。

環境

• M5StickC
• M5Atom Matrix
• MacBook Pro x86_64 macOS Big Sur 11.3
• Processing 4.0a3 (3.xはP3D表示がされませんでした)

ソースコード

M5StickCとM5Atom Matrixは基本的にどちらも同じソースコードです。
M5.IMU.getAhrsData()で姿勢角を取得してUSBシリアルとBluetooth SPPへ値を出力しています。
動作には直接関係ありませんが、ボタン(M5StickCの場合はボタンA)を押した時にデバイスをリセットするようにしています。

ProcessingはUSBシリアルでもBluetooth SPPのどちらでも動作するようになっています。
実行した時にコンソールにシリアルポート一覧が表示されますので、2台のデバイスのシリアルポートを選択します。
下記の例では、2と3がBluetooth SPPで、4と5がUSBシリアルです。

0: /dev/cu.Bluetooth-Incoming-Port
1: /dev/cu.iPhone-WirelessiAPv2
2: /dev/cu.M5AtomMatrix-ESP32SPP
3: /dev/cu.M5StickC-ESP32SPP
4: /dev/cu.usbserial-8152A83D92
5: /dev/cu.usbserial-C152AAC745
6: /dev/tty.Bluetooth-Incoming-Port
7: /dev/tty.iPhone-WirelessiAPv2
8: /dev/tty.M5AtomMatrix-ESP32SPP
9: /dev/tty.M5StickC-ESP32SPP
10: /dev/tty.usbserial-8152A83D92
11: /dev/tty.usbserial-C152AAC745

また、表示させるシューズの画像データ(orphe_left.jpg、orphe_right.jpg)はソースコードと同じディレクトリに置きます。

M5StickC

ソースコード

20210501_M5StickC_MPU6886_03.ino

# include <M5StickC.h>
# include "BluetoothSerial.h"

BluetoothSerial SerialBT;

float roll, pitch, yaw;

void setup() {
  M5.begin();
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(4);
  M5.Lcd.setCursor(5, 5);
  M5.IMU.Init();
  SerialBT.begin("M5StickC");
}

void loop() {
  M5.update();  // ボタンの状態を更新
  if (M5.BtnA.wasReleased()) {
    M5.Lcd.print('A');
    ESP.restart();
  }
  M5.IMU.getAhrsData(&pitch, &roll, &yaw);  // 姿勢角を取得
  Serial.printf("%5.1f,%5.1f,%5.1f\n", pitch, roll, yaw);
  SerialBT.printf("%5.1f, %5.1f, %5.1f\n", pitch, roll, yaw);
  delay(20);
}

M5Atom Matrix

ソースコード

20210501_M5AtomMatrix_MPU6886_02.ino

# include <M5Atom.h>
# include "BluetoothSerial.h"

BluetoothSerial SerialBT;

float roll, pitch, yaw;

void setup() {
  M5.begin();
  M5.IMU.Init();
  SerialBT.begin("M5AtomMatrix");
}

void loop() {
  M5.update();  // ボタンの状態を更新
  if (M5.Btn.wasReleased()) {
    ESP.restart();
  }
  M5.IMU.getAhrsData(&pitch, &roll, &yaw);  // 姿勢角を取得
  Serial.printf("%5.1f,%5.1f,%5.1f\n", pitch, roll, yaw);
  SerialBT.printf("%5.1f, %5.1f, %5.1f\n", pitch, roll, yaw);
  delay(20);
}

Processing

ソースコード

sketch_210501c_M5Stack_03.pde

PImage left, right;
import processing.serial.*;
Serial port1, port2;

void setup() {
  size(600, 600, P3D);
  left = loadImage("orphe_left.jpg");  // 200 x 550 for left shoe image
  right = loadImage("orphe_right.jpg");  // 200 x 550 for right shoe image
  textureMode(IMAGE);

  String[] ports = Serial.list();
  for (int i = 0; i < ports.length; i++) {
    println(i + ": " + ports[i]);
  }
  port1 = new Serial(this, ports[2], 115200);  // Serial Port for left
  port2 = new Serial(this, ports[3], 115200);  // Serial Port for right
}

String str1, str2;

void draw() {
  if (port1.available() == 0) return;
  println(port1.available());
  while (port1.available() > 0) {
    str1 = port1.readStringUntil('\n');
  }

  if (port2.available() == 0) return;
  println(port2.available());
  while (port2.available() > 0) {
    str2 = port2.readStringUntil('\n');
  }
  
  background(0);
  scale(0.5);

  // left
  String toks[] = split(trim(str1), ",");
  if (toks.length != 3) {println("left"); return;}

  float pitch = float(toks[0]);
  float roll = -float(toks[1]);
  float yaw = 180 - float(toks[2]);

  pushMatrix();
  translate(width - 200, height , 0);
  float c1 = cos(radians(roll));
  float s1 = sin(radians(roll));
  float c2 = cos(radians(pitch));
  float s2 = sin(radians(pitch));
  float c3 = cos(radians(yaw));
  float s3 = sin(radians(yaw));
  applyMatrix(c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
              -s2, c1*c2, c2*s1, 0,
              c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
              0, 0, 0, 1);
  drawM5StickC_left();
  popMatrix();
  
  // right
  toks = split(trim(str2), ",");
  if (toks.length != 3) {println("right"); return;}

  pitch = float(toks[0]);
  roll = -float(toks[1]);
  yaw = 180 - float(toks[2]);

  pushMatrix();
  translate(width + 200, height , 0);
  c1 = cos(radians(roll));
  s1 = sin(radians(roll));
  c2 = cos(radians(pitch));
  s2 = sin(radians(pitch));
  c3 = cos(radians(yaw));
  s3 = sin(radians(yaw));
  applyMatrix(c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
              -s2, c1*c2, c2*s1, 0,
              c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
              0, 0, 0, 1);
  drawM5StickC_right();
  popMatrix();
}

void drawM5StickC_left() {
  beginShape();
  texture(left);
  vertex(-100, 0, -225,   0,   0); //V1
  vertex( 100, 0, -225, 200,   0); //V2
  vertex( 100, 0,  225, 200, 550); //V3
  vertex(-100, 0,  225,   0, 550); //V4
  endShape();
}

void drawM5StickC_right() {
  beginShape();
  texture(right);
  vertex(-100, 0, -225,   0,   0); //V1
  vertex( 100, 0, -225, 200,   0); //V2
  vertex( 100, 0,  225, 200, 550); //V3
  vertex(-100, 0,  225,   0, 550); //V4
  endShape();
}

ソースコードはGithubにも置いておきます。

実行画面

動画

参考