M5ATOM S3のIMUセンサ値をSG90の角度に反映させる

M5ATOM S3 には6軸IMUセンサ（MPU6886）が搭載されています。このセンサの値を取得してサーボモータを任意の角度に動かすことで、まるでずっと弓矢に狙われ続けているような表現をすることができます。

前回記事の構成から変更ありません。
https://elchika.com/article/aa7fdaca-4ea7-4c02-a042-4bfd3635d8be/

必要なライブラリは前回記事と同様です。
https://elchika.com/article/aa7fdaca-4ea7-4c02-a042-4bfd3635d8be/

IMUのセンサ取得値をサーボモータSG90に反映する
#include <M5Unified.h> 
#include <ESP32Servo.h>

Servo servo1; // create four servo objects 
int servo1Pin = 2;

// Published values for SG90 servos; adjust if needed
int minUs = 500;
int maxUs = 2400;

float pos = 0;      // position in degrees

void setup() {
  auto cfg = M5.config();       // M5Stack初期設定用の構造体を代入
  M5.begin(cfg);                  // M5デバイスの初期化
  servo1.setPeriodHertz(50);      // Standard 50hz servo
  servo1.attach(servo1Pin, minUs, maxUs);

  //IMU_CHECK
  const char* name;
  switch (M5.Imu.getType())
  {
  case m5::imu_none:        name = "not found";   break;
  case m5::imu_sh200q:      name = "sh200q";      break;
  case m5::imu_mpu6050:     name = "mpu6050";     break;
  case m5::imu_mpu6886:     name = "mpu6886";     break;
  case m5::imu_mpu9250:     name = "mpu9250";     break;
  case m5::imu_bmi270:      name = "bmi270";      break;
  default:                  name = "unknown";     break;
  };
  M5_LOGI("imu:%s", name);

}


//Average
#define STOCK 15
int stock_p[STOCK];
int stock_p_num=0;

float CalcAverage(float num){
  stock_p_num++;
  if(stock_p_num >=STOCK){
    stock_p_num=0;
  }
  stock_p[stock_p_num]=num;
  float result=0;

  for(int i = 0;i < STOCK;i++){
    result +=stock_p[i];
  }
  result = result/(STOCK);
  return result;
}


void loop() {
  M5.update();

  // To update the IMU value, use M5.Imu.update.
  // If a new value is obtained, the return value is non-zero.
  auto imu_update = M5.Imu.update();
  if (imu_update)
  {
    // Obtain data on the current value of the IMU.
    auto data = M5.Imu.getImuData();
    //drawGraph(rect_graph_area, data);
   //Serial.println(data.accel.x);
    // The data obtained by getImuData can be used as follows.
    pos = data.accel.x;      // accel x-axis value.
    data.accel.y;      // accel y-axis value.
    data.accel.z;      // accel z-axis value.
    data.accel.value;  // accel 3values array [0]=x / [1]=y / [2]=z.

    data.gyro.x;      // gyro x-axis value.
    data.gyro.y;      // gyro y-axis value.
    data.gyro.z;      // gyro z-axis value.
    data.gyro.value;  // gyro 3values array [0]=x / [1]=y / [2]=z.

    data.mag.x;       // mag x-axis value.
    data.mag.y;       // mag y-axis value.
    data.mag.z;       // mag z-axis value.
    data.mag.value;   // mag 3values array [0]=x / [1]=y / [2]=z.

    data.value;       // all sensor 9values array [0~2]=accel / [3~5]=gyro / [6~8]=mag
    pos = pos * 100;
    pos = map(pos,-100,100,0,180);  //-100〜100のデータを0〜180°に変換
    pos = CalcAverage(pos); //平均値を求める

    Serial.print("AveragePos=");
    Serial.println(pos);
    // M5_LOGV("ax:%f  ay:%f  az:%f", data.accel.x, data.accel.y, data.accel.z);
    // M5_LOGV("gx:%f  gy:%f  gz:%f", data.gyro.x , data.gyro.y , data.gyro.z );
    // M5_LOGV("mx:%f  my:%f  mz:%f", data.mag.x  , data.mag.y  , data.mag.z  );

  }else{
      servo1.write(pos); //サーボモータの角度指定
      delay(1);
  }
}

ArduinoIDE >ファイル>スケッチ例>M5Unified>Basic>Imu からImuを使ったサンプルプログラムを使用することができます。今回のプログラムはサンプルプログラムからIMUのセンサ値を取得する部分を流用して作成しました。

posの参照を他のデータ(たとえば　data.accel.y)にすると動きが変わりますので試してみて下さい。

pos = data.accel.x;      // accel x-axis value.
    data.accel.y;      // accel y-axis value.
    data.accel.z;      // accel z-axis value.
    data.accel.value;  // accel 3values array [0]=x / [1]=y / [2]=z.

    data.gyro.x;      // gyro x-axis value.
    data.gyro.y;      // gyro y-axis value.
    data.gyro.z;      // gyro z-axis value.
    data.gyro.value;  // gyro 3values array [0]=x / [1]=y / [2]=z.

    data.mag.x;       // mag x-axis value.
    data.mag.y;       // mag y-axis value.
    data.mag.z;       // mag z-axis value.
    data.mag.value;   // mag 3values array [0]=x / [1]=y / [2]=z.