Atom Matrixで6軸ジャイロセンサーを使う

Atom Matrixでネットの情報を参考に倒立振子を作成している中、ジャイロセンサーについて本体と軸の関係が分からなかったので、センサーからの数値をLEDに変換してバー表示できるようなスケッチを作ってみました。

#include <M5Atom.h>

float accX, accY, accZ;
float gyroX, gyroY, gyroZ;

float brightness = 0.7;  // ★ 輝度30% に設定

int xy2i(int x, int y) {
  return y * 5 + x;
}

//LED色設定
uint32_t rgb(int r, int g, int b) {
  r = r * brightness;
  g = g * brightness;
  b = b * brightness;
  return ((r & 255) << 16) | ((g & 255) << 8) | (b & 255);
}

//Accデータを5段階に変換
int mapAccToLevel(float v) {
  v = constrain(v, -1.0, 1.0);
  return (int)((v + 1.0) * 2.5);
}


void setup() {
  M5.begin(true, true, true);
  M5.IMU.Init();
}


void loop() {
  M5.IMU.getAccelData(&accX, &accY, &accZ);
  M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);

  int levelX = mapAccToLevel(accX);
  int levelY = mapAccToLevel(accY);
  int levelZ = mapAccToLevel(accZ);

  M5.dis.clear();

  // X軸（左列）
  for (int i = 0; i < levelX; i++) {
    M5.dis.drawpix(xy2i(0, 4 - i), rgb(255, 0, 0));
  }

  // Y軸（中央列）
  for (int i = 0; i < levelY; i++) {
    M5.dis.drawpix(xy2i(2, 4 - i), rgb(0, 255, 0));
  }

  // Z軸（右列）
  for (int i = 0; i < levelZ; i++) {
    M5.dis.drawpix(xy2i(4, 4 - i), rgb(0, 0, 255));
  }

  delay(100);
}

これを実行すると、X軸が赤、Y軸が緑、Z軸が青のバー表示となって、AtomMatrixの傾きに応じてLEDが光ります。

モニターを正面にして前後がY軸、左右がX軸、天地がZ軸となっていました。