라이브러리는 아래 링크에서 다운로드하고 아두이노 프로그램 메뉴에서 스케치->라이브러리 포함하기->.ZIP 라이브러리 추가를 누르고, 파일선택창이 나오면 다운받은 라이브러리 파일을 선택하시면 됩니다.

라이브러리

• I2Cdev 라이브러리
• MPU6050 라이브러리

소스코드

아래는 소스코드입니다. 아두이노 IDE에 붙여넣기 하여 컴파일하고 업로드합니다.

#include <LiquidCrystal.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

LiquidCrystal lcd(7,8,9,10,11,12);//RS,E,DB4,DB5,DB6,DB7

MPU6050 mpu;

#define INTERRUPT_PIN 2  // 인터럽트 핀
#define LED_PIN 13 // 아두이노 내장 LED
bool blinkState = false;

// MPU6050 제어 및 상태표시 변수
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// 요롤피치 계산용
Quaternion q;           // [w, x, y, z]         4원법 변수
VectorFloat gravity;    // [x, y, z]            중력 벡터
float ypr[3];           // [yaw, pitch, roll]   요롤피치 변수

int intCount;

volatile bool mpuInterrupt = false;     // MPU6050 인터럽트 발생유무 확인용
// 인터럽트 루틴
void dmpDataReady() {
    mpuInterrupt = true;
}


void setup() {

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    lcd.begin(16,2);
    intCount = 0;

    Serial.begin(9600);
    while (!Serial);

    //초기화
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT);

    //MPU6050과 연결상태 확인
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    //요롤피치 산출을 위한 DMP(Digital Motion Processor) 초기화
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    //옵셋 설정
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688);

    //정상동작여부 확인 0이면 정상
    if (devStatus == 0) {
        //DMP 활성화
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        //아두이노 인터럽트 활성화
        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        //DMP 대기상태
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // DMP패킷 크기 비교
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    //LED 포트 출력으로 설정
    pinMode(LED_PIN, OUTPUT);
}

void loop() {
    // 오류시 종료
    if (!dmpReady) return;

    //MPU6050 데이터 대기
    while (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount < packetSize) {
          // try to get out of the infinite loop 
          fifoCount = mpu.getFIFOCount();
        }  
        // 사용자 프로그램은 여기에 작성
        // .

    }

    // 인터럽트 플래그를 리셋
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        if(intCount++ > 20) //LCD표시 주기 조절용
        {
          intCount = 0;
          showYPR();
        }

    }
}

void showYPR()
{
  
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        lcd.clear();
        lcd.print(" Y     P     R");
        lcd.setCursor(0,1);
        lcd.print(ypr[0] * 180/M_PI,0);
        lcd.setCursor(6,1);
        lcd.print(ypr[1] * 180/M_PI,0);
        lcd.setCursor(12,1);
        lcd.print(ypr[2] * 180/M_PI,0);
        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("\t");
        Serial.println(ypr[2] * 180/M_PI);

        // 데이터 1번 표시할 때마다 LED 상태를 바꿈
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
}