zxc852905168 zxc852905168
关注数: 34 粉丝数: 27 发帖数: 4,288 关注贴吧数: 226
是等等等等等等等等等等等等 #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(9600); // 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++; }
首页 1 2 下一页