Robofun 機器人論壇

標題: MPU6050 角度問題請教 [打印本頁]

作者: ga742112 時間: 2016-3-28 23:31
標題: MPU6050 角度問題請教
各位前輩好
小弟目前使用mpu6050 做四軸飛行器

角度值雖然得到了,但遇到了一些問題想請教

算出來的pitch值 (往角度小的方向轉動) ,快速的小小轉動一下mpu6050

發現它的數值pitch會瞬間先變大,再變回當下正確的角度值

有疑惑的是,roll的數值卻是正常的....不會有值突然增大的現象

拜託前輩們及神人們指點!!

附上程式碼

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

long LastTime, NowTime, TimeSpan;
float Ax,Ay,Az;
float X,Y,Z;                                        // angle from acclerometer
float Gyro_x, Gyro_y,Gyro_z;                // angle accleration from gyro sensor
float ix,iy,iz;
float Roll,Pitch,Yaw;

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup()
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();

}

void loop()
{

  angle();

}

void angle(){
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  Ax = ax/16384.00;
  Ay = ay/16384.00;
  Az = az/16384.00;
  X = atan(Ax/sqrt(Az*Az+Ay*Ay))*180/PI;          //angle from accelermeter
  Y = atan(Ay/sqrt(Az*Az+Ax*Ax))*180/PI;
  Z = atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/PI;

  Gyro_x = gx/131.00+6;    // get angle acceleration
  Gyro_y = gy/131.00;
  Gyro_z = gz/131.00;

  NowTime = millis(); //star to count time
  TimeSpan = NowTime - LastTime; //count time pass
  ix = 0.95*(ix + Gyro_x*TimeSpan / 1000) + 0.05*Y;          // position count by Complementary Filter
  iy = 0.94*(iy + Gyro_y*TimeSpan / 1000) + 0.06*X;
  LastTime = NowTime;

  Roll = ix
  Pitch = iy;

Serial.print("Roll : ");
Serial.print(ix);
Serial.print("Pitch : ");
Serial.println(iy);
}

作者: 超新手 時間: 2016-3-29 08:33
本帖最後由超新手於 2016-3-29 08:34 編輯

這是 Filter, 所以會有些延遲, 不算"不正常"
如果覺得輸出不穩或太慢, 你可以調整一下 Complementary Filter 的參數看看
iy 的 0.94 和 0.06 , 這兩個數字只要小於 1, 而且加起來等於 1 即可
你可以大略小調一下, 再看看是不是你要的

只是.... Gyro_x = gx/131.00+6; 為什麼要加 6?

作者: ga742112 時間: 2016-3-29 11:33
前輩你好

關於Filter前面的參數我已經有調整過,但pitch部分還是會有值突然加大的情況

作者: 水瓶裡的魚 時間: 2017-6-8 22:02
你好~可以跟你請教你的程式碼的iz計算方式嗎??

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)