淘宝 MPU6050 模块的坑

/* Get tilt angles on X and Y, and rotation angle on Z
 * Angles are given in degrees
 *
 * License: MIT
 */

#include "Wire.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;
float gyroX, gyroY, gyroZ;
float gyroXerror = 0.07;
float gyroYerror = 0.03;
float gyroZerror = 0.01;
unsigned long lastTime = 0;
unsigned long gyroDelay = 10;

void initMPU()
{
  if (!mpu.begin())
  {
    Serial.println("Failed to find MPU6050 chip");
    while (1)
    {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
}

void getGyroReadings()
{
  mpu.getEvent(&a, &g, &temp);

  float gyroX_temp = g.gyro.x;
  if (abs(gyroX_temp) > gyroXerror)
  {
    gyroX += gyroX_temp / 50.00;
  }

  float gyroY_temp = g.gyro.y;
  if (abs(gyroY_temp) > gyroYerror)
  {
    gyroY += gyroY_temp / 70.00;
  }

  float gyroZ_temp = g.gyro.z;
  if (abs(gyroZ_temp) > gyroZerror)
  {
    gyroZ += gyroZ_temp / 90.00;
  }

  Serial.print("X: ");
  Serial.print(String(gyroX));
  Serial.print("\tY: ");
  Serial.print(String(gyroY));
  Serial.print("\tZ: ");
  Serial.print(String(gyroZ));
  Serial.print("\n");
}

void setup()
{
  Serial.begin(115200);
  // 我使用自定义的I2C引脚，SDA9，SCL10
  Wire.begin(9, 10);
  initMPU();
}

void loop()
{
  if ((millis() - lastTime) > gyroDelay)
  {
    getGyroReadings();
    lastTime = millis();
  }
}

bool Adafruit_MPU6050::begin(uint8_t i2c_address, TwoWire *wire,
                             int32_t sensor_id) {
  if (i2c_dev) {
    delete i2c_dev; // remove old interface
  }

  i2c_dev = new Adafruit_I2CDevice(i2c_address, wire);

  // For boards with I2C bus power control, may need to delay to allow
  // MPU6050 to come up after initial power.
  bool mpu_found = false;
  for (uint8_t tries = 0; tries < 5; tries++) {
    mpu_found = i2c_dev->begin();
    if (mpu_found)
      break;
    delay(10);
  }
  if (!mpu_found)
    return false;
  // 读取MPU6050_WHO_AM_I
  Adafruit_BusIO_Register chip_id =
      Adafruit_BusIO_Register(i2c_dev, MPU6050_WHO_AM_I, 1);
  // 检查MPU6050_WHO_AM_I是否等于MPU6050_DEVICE_ID
  // make sure we're talking to the right chip
  if (chip_id.read() != MPU6050_DEVICE_ID) {
    return false;
  }

  return _init(sensor_id);
}

void MPU6050_Read(int address,
                  uint8_t *data) {                                                    // Read from MPU6050. Needs register address, data array
    int size = sizeof(*data);                          //
    Wire.beginTransmission(MPU6050_I2C_ADDRESS);       // Begin talking to MPU6050
    Wire.write(address);                               // Set register address
    Wire.endTransmission(false);                       // Hold the I2C-bus
    Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); // Request bytes, release I2C-bus after data read
    int i = 0;                                         //
    while (Wire.available()) {                          //
        data[i++] = Wire.read(); // Add data to array
    }
}


void WhoAmI() {
    uint8_t waiByte;                                 // Data will go here
    MPU6050_Read(MPU6050_WHO_AM_I, &waiByte);        // Get data
    Serial.print(F("Device WhoAmI reports as: 0x")); //
    Serial.println(waiByte, HEX);                    // Report WhoAmI data
}