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[Ω]を接続している。

<syntaxhighlight lang="c++">
#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 );
      }
   }
}
</source>