MPU6886 3Dモデル制御その２

M5Stack 2022年4月9日

　出典：パソコン上のM5Stackの3Dモデルを、M5Stackの動きに合わせて動かす
　（１）パソコン上にM5Stackの3Dモデルを作成
　（２）IMUを使い端末の姿勢を計算
　（３）姿勢データをパソコンに送り、3Dモデルを制御

　続きの（３）の手順から始める。

姿勢データをパソコンにBlueToothで送信

　前回は、MPU6886なのでyawの値の計算値がおかしく、途中であきらめたが、ローリング、ピッチングだけでも3Dモデルを制御してみることに。
　やっているのは出典サイトの内容をなぞっただけ。
　Bluetooth経由でパソコンとシリアル通信を行うため、赤字部分を追加している。

M5C2_MPU6886_Serialプロジェクト

　VScode＋PlatFormIOの環境で、M5C2へ書き込む。

main.cpp

#include <M5Core2.h>
#include <BluetoothSerial.h>
BluetoothSerial SerialBT;
float accX = 0.0F;
float accY = 0.0F;  
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;
float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

void setup(){
  M5.begin();
  SerialBT.begin("M5StackCore2"); //Bluetoothデバイス名
  //パソコン側で「M5StackCore2」の名前のデバイスとペアリングをすること。
  M5.IMU.Init();
  M5.Lcd.fillScreen(BLACK); 
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
}

void loop() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("gyroX,  gyroY,  gyroZ");
  M5.Lcd.setCursor(0, 42);
  M5.Lcd.printf("%6.2f %6.2f%6.2f o/s", gyroX, gyroY, gyroZ);
  M5.Lcd.setCursor(0, 70);
  M5.Lcd.printf("accX,   accY,  accZ");
  M5.Lcd.setCursor(0, 92);
  M5.Lcd.printf("%5.2f  %5.2f  %5.2f G", accX, accY, accZ);
  M5.Lcd.setCursor(0, 120);
  M5.Lcd.printf("pitch,  roll,  yaw");
  M5.Lcd.setCursor(0, 142);
  M5.Lcd.printf("%5.2f  %5.2f  %5.2f deg", pitch, roll, yaw);
  Serial.printf("%5.2f,%5.2f,%5.2f\r\n", pitch, roll, yaw);
  SerialBT.printf("%5.2f,%5.2f,%5.2f\r\n", pitch, roll, yaw);
  delay(10);
}

3Dモデルを制御するProcessing側スケッチ

M5Core2_Serialスケッチ

M5Core2_Serial.pde

PImage front, back, right, left, top, bottom;
import processing.serial.*;
Serial port;
void setup() {
  size(1200, 800, P3D);
  front = loadImage("front.jpg");
  back = loadImage("back.jpg");
  right = loadImage("right.jpg");
  left = loadImage("left.jpg");
  top = loadImage("top.jpg");
  bottom = loadImage("bottom.jpg");
  textureMode(IMAGE);
  String[] ports = Serial.list(); 
 //BlueToothの通信COMポートを探す必要があるので、確認にはTeratermが便利。
  for (int i = 0; i < ports.length; i++) {
    println(i + ": " + ports[i]);
  }
　//たまたま自分の環境ではports[4]だったが、パソコンとのUSB
　　接続ケーブルを外すと、ports[3]に変わるので結構面倒。
  port = new Serial(this, ports[4], 115200);
}

void draw() {
  if (port.available() == 0) return;
  String str = port.readStringUntil('\n');
  if (str == null) return;
  String toks[] = split(trim(str), ",");
  if (toks.length != 3) return;
  float pitch = float(toks[0]);
  float roll = float(toks[1]); 
  float yaw = 180 - float(toks[2]); //出典に合わせただけ？
  print(yaw); print(", ");
  print(pitch); print(", ");
  println(roll);
  background(0);
  translate(width / 2, height / 2);
  scale(0.4);
  pushMatrix();
  float cr = cos(radians(roll));
  float sr = sin(radians(roll));
  float cp = cos(radians(pitch));
  float sp = sin(radians(pitch));
  float cy = cos(radians(yaw));
  float sy = sin(radians(yaw));
//rolling
//  applyMatrix( 1,   0,  0,  0,
//               0,  cr, sr,  0,
//               0, -sr, cr,  0,
//               0,   0,  0,  1);
//pitching
//  applyMatrix( cp, -sp, 0, 0,
//               sp,  cp, 0, 0,
//                0,   0, 1, 0,
//                0,   0, 0, 1);              
//yawing
//  applyMatrix( cy, 0, -sy, 0,
//                0, 1,  0, 0,
//              sy, 0, cy, 0,
//                0, 0,  0, 1);
//pitching＜rolling
//   applyMatrix(cp, -sp*cr, -sp*sr, 0,
//               sp, cp*cr, cp*sr, 0,
//               0, -sr, cr, 0,
//               0,  0, 0, 1);
//yawing＜pitching＜rolling               
   applyMatrix(cy*cp, -cy*sp*cr+sy*sr, -cy*sp*sr-sy*cr, 0,
               sp, cp*cr, cp*sr, 0,
               sy*cp, -sy*sp*cr-cy*sr, -sy*sp*sr+cy*cr, 0,
               0,  0, 0, 1);        
　drawM5Core2();
  popMatrix();
//  saveFrame("frames/######.png");
}

void drawM5Core2() {
//立方体各面の、左上をV1としコの字形にV1,V2,V3,V4の順に各頂点の座標を求めている。
//画像のUV座標はV1を原点（0,0）とし、横U軸側右に700、縦V軸側下に300のように決めている。
  beginShape();
  texture(front);
  vertex(-350, -150,  350,   0,   0); //V1
  vertex( 350, -150,  350, 700,   0); //V2
  vertex( 350,  150,  350, 700, 300); //V3
  vertex(-350,  150,  350,   0, 300); //V4
  endShape();
  beginShape();
  texture(back);
  vertex( 350, -150, -350,   0,   0); //V1
  vertex(-350, -150, -350, 700,   0); //V2
  vertex(-350,  150, -350, 700, 300); //V3
  vertex( 350,  150, -350,   0, 300); //V4
  endShape();
  beginShape();
  texture(right);
  vertex( 350,  -150,  350,   0,   0); //V1
  vertex( 350,  -150, -350, 700,   0); //V2
  vertex( 350,   150, -350, 700, 300); //V3
  vertex( 350,   150,  350,   0, 300); //V4
  endShape();
  beginShape();
  texture(left);
  vertex(-350,  -150, -350,   0,   0); //V1
  vertex(-350,  -150,  350, 700,   0); //V2
  vertex(-350,   150,  350, 700, 300); //V3
  vertex(-350,   150, -350,   0, 300); //V4
  endShape();
  beginShape();
  texture(top);
  vertex(-350,  -150, -350,   0,   0); //V1
  vertex( 350,  -150, -350, 700,   0); //V2
  vertex( 350,  -150,  350, 700, 700); //V3
  vertex(-350,  -150,  350,   0, 700); //V4
  endShape();
  beginShape();
  texture(bottom);
  vertex( 350,   150, -350,   0,   0); //V1
  vertex(-350,   150, -350, 700,   0); //V2
  vertex(-350,   150,  350, 700, 700); //V3
  vertex( 350,   150,  350,   0, 700); //V4
  endShape();
}