M5 Stack에서 IMU 유닛 사용

어쨌든 샘플 코드로 한번 돌려보도록 하겠습니다.

이번에는 아두노IDE로 실험을 진행하겠습니다.
M5 Stack이 공식 발표한 UiFlow-IIDE도 이용할 수 있지만, 아이콘이 잘 쓰이지 않고 처리도 느려 싫어한다.
ArduinoIDE의 스케치 예제에서 IMU까지.ino를 선택하고 실행합니다.

// define must ahead #include <M5Stack.h>
#define M5STACK_MPU6886 
// #define M5STACK_MPU9250 
// #define M5STACK_MPU6050
// #define M5STACK_200Q

#include <M5Stack.h>

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;

float temp = 0.0F;

// the setup routine runs once when M5Stack starts up
void setup(){

  // Initialize the M5Stack object
  M5.begin();
  /*
    Power chip connected to gpio21, gpio22, I2C device
    Set battery charging voltage and current
    If used battery, please call this function in your project
  */
  M5.Power.begin();
    
  M5.IMU.Init();

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
}

// the loop routine runs over and over again forever
void loop() {
    // put your main code here, to run repeatedly:
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  M5.IMU.getTempData(&temp);
  
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
  M5.Lcd.setCursor(220, 42);
  M5.Lcd.print(" o/s");
  M5.Lcd.setCursor(0, 65);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ);
  M5.Lcd.setCursor(220, 87);
  M5.Lcd.print(" G");
  M5.Lcd.setCursor(0, 110);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", pitch, roll, yaw);
  M5.Lcd.setCursor(220, 132);
  M5.Lcd.print(" degree");
  M5.Lcd.setCursor(0, 155);
  M5.Lcd.printf("Temperature : %.2f C", temp);

  delay(1);
}

가격을 대충 봤습니다.

그러나, 이degree의yaw각 표류를 토대로 회전의 속도에 따라 변화하는 값도 매우 큰 변화가 있다

코너 만들기

야w각 이외의 값은 한눈에 정확해 보이기 때문에 이번에는 야w각만 만드는 프로그램을 스스로 만들기로 했다.

#define M5STACK_MPU6886
#define CALIBCOUNT 10000

#include <M5Stack.h>

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 yaw   = 0.0F;

float gyroOffsetZ = 0.0;

float preTime = 0.0F;
float dt = 0.0F;

float pregz = 0.0F;
float degree = 0;

int cnt = 0;

void calibration()
{
  delay(1000);
  M5.Lcd.printf("...");
  float gyroSumZ = 0;
  int count = CALIBCOUNT;
  for (int i = 0; i < count; i++) {
    M5.update();

    float gyroZ;
    M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);

    gyroSumZ += gyroZ;
    if (M5.BtnB.wasPressed())
    {
      M5.Lcd.clear();
      M5.Lcd.setCursor(140, 120);
      M5.Lcd.printf("Exit");
      delay(500);
      return;
    }
  }
  gyroOffsetZ = gyroSumZ / count - 0.02;
  M5.Lcd.clear();
  M5.Lcd.setCursor(140, 120);
  M5.Lcd.printf("Done");
  delay(500);
}

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

  gyroZ -= gyroOffsetZ;

  dt = (micros() - preTime) / 1000000;
  preTime = micros();

  yaw -= (pregz + gyroZ) * dt / 2;
  pregz = gyroZ;

  if(yaw > 180)
  {
    yaw -= 360;
  }
  else if(yaw < -180)
  {
    yaw += 360;
  }
  delay(10);
}

void Button()
{
  M5.update();
  if (M5.BtnA.wasPressed())
  {
    cnt--;
    M5.Lcd.clear();
  }

  if (M5.BtnC.wasPressed())
  {
    cnt++;
    M5.Lcd.clear();
  }
}

void ResetGyro()
{
  gyroZ = 0.0;
  pregz = 0.0;
  yaw = 0.0;
  M5.Lcd.clear();
  M5.Lcd.setCursor(120, 120);
  M5.Lcd.printf("RESET");
  delay(500);
  M5.Lcd.clear();
}

void Main()
{
  M5.Lcd.clear();
  while (true)
  {
    M5.update();
    M5.Lcd.fillCircle(160 + 80 * cos(degree), 120 + 80 * sin(degree), 10, BLACK);
    M5.Lcd.setCursor(160, 0);
    degree = (yaw - 90) / (180 / PI);
    GetGyro();
    M5.Lcd.drawCircle(160, 120, 80, WHITE);
    M5.Lcd.fillCircle(160 + 80 * cos(degree), 120 + 80 * sin(degree), 10, GREEN);
    M5.Lcd.printf("%4.0f", yaw);
    if (M5.BtnB.wasPressed())
    {
      M5.Lcd.clear();
      break;
    }
  }
}

void setup() {

  M5.begin();


  M5.Power.begin();

  M5.IMU.Init();

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(WHITE , BLACK);
  M5.Lcd.setTextSize(2);
  delay(1);
}


void loop() {
  Button();

  switch (cnt)
  {
    case 0:
      M5.Lcd.setCursor(140, 120);
      M5.Lcd.printf("Main");
      if (M5.BtnB.wasPressed())
      {
        Main();
      }
      break;
    case 1:
      M5.Lcd.setCursor(90, 120);
      M5.Lcd.printf("Calibration");
      if (M5.BtnB.wasPressed())
      {
        calibration();
      }
      break;
    case 2:
      M5.Lcd.setCursor(100, 120);
      M5.Lcd.printf("ResetGyro");
      if (M5.BtnB.wasPressed())
      {
        ResetGyro();
      }
      break;
    default:
      cnt = 0;
      break;
  }
}

프로그램이 길어졌어요.💦
시간이 있어서 여분을 추가했습니다.같은 프로그램에서 교정과 초기 방향의 리셋, 그리고 실제 yaw각 변화의 가시화를 진행할 수 있다.

정렬 함수 Calibration

Calibration의 함수는 맨 위 define의 수량인 CALIBCCOUNT의 데이터만 추출하여 평균값을 편이값에 대입하는 것입니다.교정 중에 IMU를 이동하지 않는 것이 전제 조건입니다.

각도 변화 함수 GetGyro

GetGyro에서는 세로축 각속도(deg/s), 가로축 시간(s)으로 구성된 도표의 면적을 누적하여 각도 변화를 구한다.(포인트는 사다리꼴로 누적하는 방법을 사용합니다. 자세한 내용은 본 사이트를 참고하십시오.)
https://garchiving.com/angular-from-angular-acceleration/
실제로 야우에 들어간 값은 -180~180이다.

팽이 방향 초기화 함수 ResetGyro

ResetGyro는 yaw 각도를 찾기 위해 필요한 매개 변수와 yaw 각도 자체를 초기화하는 간단한 것이다.

마지막으로 함수 Main은 확정된 yaw 각도의 변화를 가시화합니다

이번에 마지막으로 하고 싶은 일은 각도 변화를 시각화하는 것이어서 메인이라고 이름을 지었다.
이 함수는 실제로 원에 GetGyro에서 얻은 값을 그려서 시각화합니다.원의 점의 좌표는 반지름이 x임을 나타냅니다.θyaw각으로 설정된 각도 변화θ,xsinθ).따라서 최종적으로 화면 중앙에 반경 x의 원을 그린다(원의 중심 x 좌표 + (xcos)θ),중심점 y 좌표+(xsin)θ))를 참고하십시오.
하지만 한 번 그린 점은 자동으로 사라지지 않기 때문에 매번 다음 순환에서 그 점을 검은색으로 칠한다.따라서 표시와 사라지는 동작을 반복하면 점이 깜박거려서 잘 보이지 않는데 좋은 방법이 있나요?