Arduino Mega 2560 LESSON:16 GY-521 MEMS加速度計・MEMSジャイロモジュール

Arduino Mega 2560に戻る。

概要

　GY-521 MEMS加速度計・MEMSジャイロモジュールを接続するレッスンです。MEMS(Micro Electro Mechanical Systems)とは、半導体チップの中に、非常に小さな機械的構造を作り込む技術です。I^2C:Inter-Integrated Circuit(アイスクエアシー:集積回路間通信)のSDAピンSerial DataとSCLピンSerialClockを使う配線です。

　完成図

　コードは以下のようなものです。

// MPU-6050 Short Example Sketch
//www.elegoo.com
//2016.12.9

#include<Wire.h>
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float velocityX,velocityY,velocityZ
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  //16384で1g(重力加速度)、1g(重力加速度)は9.80665[m/s^2] 
  Serial.print(" | AcX = "); Serial.print(AcX/16384.00*9.80665);
  Serial.print(" | AcY = "); Serial.print(AcY/16384.00*9.80665);
  Serial.print(" | AcZ = "); Serial.print(AcZ/16384.00*9.80665);
  Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  //equation for temperature in degrees C from datasheet
  //32768で250度/s、1度/sは131.072
  Serial.print(" | GyX = "); Serial.print(GyX / 131.0);
  Serial.print(" | GyY = "); Serial.print(GyY / 131.0);
  Serial.print(" | GyZ = "); Serial.print(GyZ / 131.0);

  Serial.println();
  delay(333);
}

　角度の積算をするコードは以下のようなものです。

#include <Wire.h>

const int MPU_addr = 0x68;

int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

// 角速度 [度/秒]
float velocityX, velocityY, velocityZ;

// 積算した回転角度 [度]
float angleX = 0;
float angleY = 0;
float angleZ = 0;

// 静止時のジャイロ補正値 [度/秒]
float offsetX = 0;
float offsetY = 0;
float offsetZ = 0;

// 前回測定時刻 [マイクロ秒]
unsigned long previousTime = 0;
unsigned long previousPrintTime = 0;

// 14バイトのセンサーデータを読み取る
bool readMPU() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;

  if (Wire.requestFrom(MPU_addr, 14, true) != 14) {
    return false;
  }

  AcX = (int16_t)((Wire.read() << 8) | Wire.read());
  AcY = (int16_t)((Wire.read() << 8) | Wire.read());
  AcZ = (int16_t)((Wire.read() << 8) | Wire.read());
  Tmp = (int16_t)((Wire.read() << 8) | Wire.read());
  GyX = (int16_t)((Wire.read() << 8) | Wire.read());
  GyY = (int16_t)((Wire.read() << 8) | Wire.read());
  GyZ = (int16_t)((Wire.read() << 8) | Wire.read());

  return true;
}

void setup() {
  Wire.begin();
  Serial.begin(9600);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1
  Wire.write(0);     // スリープ解除
  Wire.endTransmission(true);

  // ジャイロ測定範囲を ±250度/秒に設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);  // GYRO_CONFIG
  Wire.write(0x00);
  Wire.endTransmission(true);

  delay(500);

  // 起動時の静止状態を測定してゼロ点補正
  Serial.println("Keep MPU-6050 still. Calibrating...");

  const int samples = 500;
  float sumX = 0;
  float sumY = 0;
  float sumZ = 0;

  for (int i = 0; i < samples; i++) {
    if (readMPU()) {
      sumX += GyX / 131.0;
      sumY += GyY / 131.0;
      sumZ += GyZ / 131.0;
    } else {
      i--;  // 読み取り失敗時は再試行
    }
    delay(3);
  }

  offsetX = sumX / samples;
  offsetY = sumY / samples;
  offsetZ = sumZ / samples;

  Serial.println("Calibration complete.");
  Serial.println("Start rotating the sensor.");

  previousTime = micros();
  previousPrintTime = millis();
}

void loop() {
  if (!readMPU()) return;

  unsigned long currentTime = micros();

  // 前回からの経過時間 [秒]
  float dt = (currentTime - previousTime) / 1000000.0;
  previousTime = currentTime;

  // 角速度 [度/秒]（静止時のずれを補正）
  velocityX = GyX / 131.0 - offsetX;
  velocityY = GyY / 131.0 - offsetY;
  velocityZ = GyZ / 131.0 - offsetZ;

  // 角速度 × 経過時間 = 回転角度の増分
  angleX += velocityX * dt;
  angleY += velocityY * dt;
  angleZ += velocityZ * dt;

  // 表示だけ333ms間隔にする
  if (millis() - previousPrintTime >= 333) {
    previousPrintTime = millis();

    Serial.print("GyX = ");
    Serial.print(velocityX, 2);
    Serial.print(" | GyY = ");
    Serial.print(velocityY, 2);
    Serial.print(" | GyZ = ");
    Serial.print(velocityZ, 2);

    Serial.print(" | AngleX = ");
    Serial.print(angleX, 2);
    Serial.print(" | AngleY = ");
    Serial.print(angleY, 2);
    Serial.print(" | AngleZ = ");
    Serial.println(angleZ, 2);
  }
}

　各軸の関係は以下の図の通りです。

AIによる作図

　3D位置の慣性航法のような計算をするのは、重力のXYZ方向への比重を計算し続ける必要がありこれよりはるかに複雑になるので、さすがにこの程度の勉強ページとしてはボリュームが大きくなりすぎるので割愛。

　

Arduino Mega 2560に戻る。