Arduinoの基礎 - 3軸加速度センサとジャイロモジュール

概要

GY-521は、InvenSense社のMPU-6050を利用するためのブレイクアウトボード(センサボード)である。
MPU-6050は、3軸ジャイロと3軸加速度センサおよびDMP(デジタル・モーション・プロセッサ)を搭載しており、センサの値をDMPが処理してモーショントラッキングを実現している。

MPU-6050の動作電圧は3.3[V]であるが、GY-521にはレギュレータが搭載されているので、5[V]でも利用できる。
Arduinoとのインターフェイスは、I2Cを使用する。

GY-521の使用方法

Jeff Rowberg氏が作成したI2Cdevlibライブラリを使用すると便利である。
https://github.com/jrowberg/i2cdevlib

ダウンロードしたファイルを解凍して、その中にあるI2CdevディレｋトリとMPU6050ディレクトリを~/Arduino/librariesディレクトリにコピーする。
(Arduino IDEで追加しても認識されない可能性があるため)

GY-521ボードを見ると、X軸とY軸の方向が記載されている。
回転角度は、それぞれの軸の向きで右ネジの進む方向が正になる。
X軸まわりとY軸まわりの動作を、次のセクションで動作させながら確認する。

サンプルコード

下図のように、GY-521をブレッドボードに取り付けて、LEDを4方に配置してブレッドボードを傾けた方向のLEDを点灯させる。

また、GY-521とLEDは下図のように配置する。
飛行機に例えると、X軸方向が機首の向いている方向になる。X軸まわりの回転角がロール角で、Y軸まわりがピッチ角となる。
ロール角およびピッチ角を確認して、30度より下がった時に下がった向きのLEDを点灯させる。

ArduinoとGY-521ボードは、I2Cインターフェイスで接続する。Arduino UNOのI2Cインターフェイスのピン配置は、SDAがA4、SCLがA5である。
また、I2Cではプルアップ抵抗として、10[kΩ]をSCL、SDAに接続している。
ちなみに、I2Cバスのアドレスは、AD0がLowでb1101000、Highでb1101001である。

VCCは5[V]に接続する。

GY-521のINT(割り込み)ピンは、Arduino UNOのD2に接続する。
LED1〜4は、Arduino UnoのD8〜D11に接続する。電流制限抵抗として、220[Ω]を接続している。

 #include "I2Cdev.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "Wire.h"
 
 const int PIN_INTERRUPT = 2;
 const int PIN_LED1 = 8;
 const int PIN_LED2 = 9;
 const int PIN_LED3 = 10;
 const int PIN_LED4 = 11;
 
 MPU6050 mpu;
 
 bool dmpReady = false;
 uint8_t mpuIntStatus;
 uint8_t devStatus;
 uint16_t packetSize;
 uint16_t fifoCount;
 uint8_t fifoBuffer[64];
 
 Quaternion q;
 VectorFloat gravity;
 float ypr[3];
 
 volatile bool mpuInterrupt = false;
 
 void dmpDataReady()
 {
    mpuInterrupt = true;
 }
 
 void setup()
 {
    Wire.begin();
    Wire.setClock(400000);
 
    mpu.initialize();
    pinMode(PIN_INTERRUPT, INPUT);
 
    while(!mpu.testConnection())
    {
 
    }
 
    devStatus = mpu.dmpInitialize();
 
    if (devStatus == 0)
    {
       mpu.setDMPEnabled(true);
       attachInterrupt(digitalPinToInterrupt(PIN_INTERRUPT), dmpDataReady, RISING);
       mpuIntStatus = mpu.getIntStatus();
       dmpReady = true;
       packetSize = mpu.dmpGetFIFOPacketSize();
    }
 
    pinMode( PIN_LED1, OUTPUT);
    pinMode( PIN_LED2, OUTPUT);
    pinMode( PIN_LED3, OUTPUT);
    pinMode( PIN_LED4, OUTPUT);
 
    digitalWrite( PIN_LED1, LOW );
    digitalWrite( PIN_LED2, LOW );
    digitalWrite( PIN_LED3, LOW );
    digitalWrite( PIN_LED4, LOW ); 
 }
 
 void loop()
 {
    if (!dmpReady)
    {
       return;
    }
 
    while (!mpuInterrupt && fifoCount < packetSize)
    {
 
    }
 
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
 
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
       mpu.resetFIFO();
    }
    else if (mpuIntStatus & 0x02)
    {
       while (fifoCount < packetSize)
       {
          fifoCount = mpu.getFIFOCount();
       }
 
       mpu.getFIFOBytes(fifoBuffer, packetSize);        
       fifoCount -= packetSize;
 
       mpu.dmpGetQuaternion(&q, fifoBuffer);
       mpu.dmpGetGravity(&gravity, &q);
       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 
       float pitch = ypr[1] * 180/M_PI;
       float roll = ypr[2] * 180/M_PI;
 
       // ROLL
       if( 30 < roll && roll < 150 )
       {
          digitalWrite( PIN_LED2, HIGH );
          digitalWrite( PIN_LED4, LOW );
       }
       else if( -150 < roll && roll < -30 )
       {
          digitalWrite( PIN_LED4, HIGH );
          digitalWrite( PIN_LED2, LOW );
       }
       else
       {
          digitalWrite( PIN_LED2, LOW );
          digitalWrite( PIN_LED4, LOW );
       }
 
       // PITCH
       if( 30 < pitch && pitch < 150 )
       {
          digitalWrite( PIN_LED3, HIGH );
          digitalWrite( PIN_LED1, LOW );
       }
       else if( -150 < pitch && pitch < -30 )
       {
          digitalWrite( PIN_LED1, HIGH );
          digitalWrite( PIN_LED3, LOW );
       }
       else
       {
          digitalWrite( PIN_LED1, LOW );
          digitalWrite( PIN_LED3, LOW );
       }
    }
 }