範例九-讀取陀螺儀資料

I2C 介面只需兩根 SCL/SDA 腳位，就可以接不同的裝置 (裝置間 address 要不同)

-> 使用陀螺儀時，請先校正後再填入校正值。本範例不含校正的程式.

// 範例九:

// I2C 同時插上 SSD1306 及 MPU6050 陀螺儀，讓陀螺儀資料顯示在 OLED 上

// (陀螺儀使用時，請先校正)

// 使用 I2C 腳位 GPIO21/22

// 需要的 library : I2Cdev，MPU6050_6Axis_MotionApps20，U8g2lib

// FB : https://www.facebook.com/mason.chen.1420

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

#include "U8g2lib.h"

U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

MPU6050 mpu;

int MPUOffsets[6] = {0, 0, 0, 0, 0, 0}; //更新 MPU6050 的校正值

#define M_PI 3.14159265;

#define debug

// MPU control/status vars

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

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorFloat gravity; // [x, y, z] gravity vector

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

bool run_status = true;

double input, out_y,out_p,out_r;

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void dmpDataReady() {

mpuInterrupt = true;

}

/** Get latest byte from FIFO buffer no matter how much time has passed.

=== GetCurrentFIFOPacket ===

================================================================

Returns 1) when data was received

0) when no valid data is available

================================================================ */

uint8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof

uint16_t fifoC;

if ((fifoC = mpu.getFIFOCount()) > length) {

while (fifoC > length) { // Shrink down to 1 packet

mpu.getFIFOBytes(data, length);

fifoC--;

}

if ((fifoC = mpu.getFIFOCount()) > length) {

while (fifoC > length) { // We got more data while we were shrinking

mpu.getFIFOBytes(data, length);

fifoC--;

}

if ((fifoC = mpu.getFIFOCount()) > length) {

while (fifoC > length) {// we need to be sure, but this shouldn't happen... Shrink some more

mpu.getFIFOBytes(data, length);

fifoC--;

}

}

}

}

while (fifoC && (fifoC < length)) fifoC = mpu.getFIFOCount(); //Finish loading the packet!

if (fifoC == length) {

mpu.getFIFOBytes(data, length);

return 1;

}

return 0;

}

void setup() {

#ifdef debug

Serial.begin(115200);

#endif

// join I2C bus (I2Cdev library doesn't do this automatically)

Wire.begin();

Wire.setClock(400000);

u8g2.begin();

u8g2.setFlipMode(1); // 畫面轉 180 度

u8g2.setFont(u8g2_font_6x10_tf);

u8g2.setFontRefHeightExtendedText();

u8g2.setDrawColor(1);

u8g2.setFontPosTop();

u8g2.setFontDirection(0);

u8g2.setFont(u8g2_font_8x13_mf);

u8g2.firstPage();

do {

u8g2.drawStr(0, 0, String("MPU test!").c_str());

u8g2.drawLine(0, 11, 60, 11);

} while (u8g2.nextPage());

#ifdef debug

Serial.println(F("Initializing I2C devices..."));

#endif

mpu.initialize();

// verify connection

#ifdef debug

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

#endif

// load and configure the DMP

devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity

mpu.setXAccelOffset(MPUOffsets[0]);

mpu.setYAccelOffset(MPUOffsets[1]);

mpu.setZAccelOffset(MPUOffsets[2]);

mpu.setXGyroOffset(MPUOffsets[3]);

mpu.setYGyroOffset(MPUOffsets[4]);

mpu.setZGyroOffset(MPUOffsets[5]);

if (devStatus == 0) {

#ifdef debug

Serial.println(F("Enabling DMP..."));

#endif

mpu.setDMPEnabled(true);

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();

} else {

#ifdef debug

Serial.print("DMP Initialization failed ");

#endif

}

}

void loop() {

if (! GetCurrentFIFOPacket(fifoBuffer, packetSize))return;

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q

mpu.dmpGetGravity(&gravity, &q); //get value for gravity

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

out_y = ypr[0] * 180/M_PI;

out_p = ypr[1] * 180/M_PI;

out_r = ypr[2] * 180/M_PI;

#ifdef debug

Serial.print("Yaw =");

Serial.print(out_y);

Serial.print(",Pitch =");

Serial.print(out_p);

Serial.print(",Roll =");

Serial.println(out_r);

#endif

u8g2.firstPage();

do {

u8g2.drawStr(0, 0, "Yaw =");

u8g2.drawStr(60, 0, String(out_y).c_str());

u8g2.drawStr(0, 16, "Pitch =");

u8g2.drawStr(60, 16, String(out_p).c_str());

u8g2.drawStr(0, 32, "Roll =");

u8g2.drawStr(60, 32, String(out_r).c_str());

} while (u8g2.nextPage());

}