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());
}