//这个程序是读取了MPU6050的初始数据(未标定)
//并以此计算由加速度计得到的pitch和由陀螺仪得到的pitch
#include <Wire.h>
const int MPU=0x68;//MPU6050 I2Caddress
float AccX,AccY,AccZ;
float GyroX,GyroY,GyroZ;
float Acc_pitch,Gyr_pitch;
float elapsedTime,currentTime,previousTime;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Wire.begin();
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset -place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
}
void loop() {
// = = = Read accleromter data = = = //
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); //Read 6 reqisters total,each axis value is stored in .....
//For a range of +-2g,we need to divide the raw values by 16384,according to the datas.....
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
// = = = Read gyroscope data = = = //
previousTime = currentTime; //上一次的时间存于此处
currentTime = millis(); //本次到达此处的新时间点
elapsedTime = (currentTime - previousTime) / 1000; //本次与上一次的时间差转化为以秒为单位
Wire.beginTransmission(MPU);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 4 registers total,each axis value is stored in.....
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s rang we have we have to div.....
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// = = = 计算由加速度计得来的pitch = = = //
Acc_pitch = atan(AccY / AccZ) * 180 / PI;
// = = = 计算由陀螺仪得来的pitch = = = //
Gyr_pitch = Gyr_pitch + GyroX * elapsedTime;
// Print the values on the serial monitor
Serial.print(Acc_pitch);
Serial.print("\t");
Serial.print(Gyr_pitch);
Serial.print("\t");
Serial.println(elapsedTime);
delay(5);
} |