arduino控制GY521源代码

arduino控制GY521源代码

//从串口得到GY521 MPU6050的数据
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

float C_Z = -1343.91;//Z轴零点偏移量
float C_Gyro = -99.90;//陀螺仪零点偏移量
float Z_Min = -17873.76;//最小极值
float Z_Max = 15186.91;//最大极值
float T_Z = 3;//Z轴角度补偿时间常数
float R_Z = 180/(Z_Max - Z_Min);//Z轴比例
float R_Gyro = 0.081;//陀螺仪比例
unsigned long T_Now =0;//系统当前时间
unsigned long T_Last;//上次时间
float Angle_G,Angle_AG,Angle_GG;
int i;
float GYRO;

void setup() {
Wire.begin();
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取6050数据
if (i>20)
{
GYRO = GYRO/20;
float Angle_Z = (az-C_Z)*R_Z;//加速度计 角度计算 （读取值-偏移量）*比例 单位：°
Angle_G = -(GYRO-C_Gyro)*R_Gyro;//陀螺仪采样 （采样值-偏移量）*比例 单位：°/s
Angle_AG = Angle_AG + (((Angle_Z-Angle_AG)*1/T_Z)+Angle_G)*0.005;//滤波
Angle_GG = Angle_GG + Angle_G*0.005;//陀螺仪对X轴积分 得出角度。
Serial.print(Angle_Z);
Serial.print(",");
Serial.print(Angle_GG);
Serial.print(",");
Serial.print(Angle_AG);
Serial.print("\n");
i=0;
}
GYRO = GYRO + gx;
i++;
}

如以上回答内容为低俗、色情、不良、暴力、侵权、涉及违法等信息，可以点下面链接进行举报！

点此我要举报以上问答信息