Arduino+MPU 6050 学习记录

pijunkuan 发表于 2014-12-6 19:07:17

本帖最后由 pijunkuan 于 2014-12-7 00:39 编辑

研究了mpu 6050 一个多月了，做了一个简单的展示上位机，对mpu传回来的数据做了一点小小的处理，废话不多说了，这里直接上程序，希望各位高手们多多指点一下，个人感觉程序里面有不少的bug，但是自己是学机械的，对电子和软件一窍不通，一直处于自学阶段，望得到高手们的帮助。
坐等各位高手们的回复。

Arduino程序：
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;
#define LED_PIN 13

unsigned long last_read_time;
float       last_x_angle;// These are the filtered angles
float       last_y_angle;
float       last_z_angle;
float       last_gyro_x_angle;// Store the gyro angles to compare drift
float       last_gyro_y_angle;
float       last_gyro_z_angle;

void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro)
{
last_read_time = time;
last_x_angle = x;
last_y_angle = y;
last_z_angle = z;
last_gyro_x_angle = x_gyro;
last_gyro_y_angle = y_gyro;
last_gyro_z_angle = z_gyro;
}

inline unsigned long get_last_time() {return last_read_time;}
inline float get_last_x_angle() {return last_x_angle;}
inline float get_last_y_angle() {return last_y_angle;}
inline float get_last_z_angle() {return last_z_angle;}
inline float get_last_gyro_x_angle() {return last_gyro_x_angle;}
inline float get_last_gyro_y_angle() {return last_gyro_y_angle;}
inline float get_last_gyro_z_angle() {return last_gyro_z_angle;}

float base_x_gyro = 0;
float base_y_gyro = 0;
float base_z_gyro = 0;
float base_x_accel = 0;
float base_y_accel = 0;
float base_z_accel = 0;

float GYRO_FACTOR;
float ACCEL_FACTOR;

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

char dataOut;

void calibrate_sensors() {
int    num_readings = 10;

// Discard the first reading (don't know if this is needed or
// not, however, it won't hurt.)
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// Read and average the raw values
for (int i = 0; i < num_readings; i++) {
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
base_x_gyro += gx;
base_y_gyro += gy;
base_z_gyro += gz;
base_x_accel += ax;
base_y_accel += ay;
base_y_accel += az;
}

base_x_gyro /= num_readings;
base_y_gyro /= num_readings;
base_z_gyro /= num_readings;
base_x_accel /= num_readings;
base_y_accel /= num_readings;
base_z_accel /= num_readings;
}

void setup() {
Wire.begin();
Serial.begin(57600);
while (!Serial);
mpu.initialize();
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

pinMode(LED_PIN, OUTPUT);

calibrate_sensors();
set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
}

void loop()
{
const float RADIANS_TO_DEGREES = 57.2958; //180/3.14159
unsigned long t_now = millis();
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

float gyro_x = (gx - base_x_gyro)/131.0;
float gyro_y = (gy - base_y_gyro)/131.0;
float gyro_z = (gz - base_z_gyro)/131.0;
float accel_x = ax; // - base_x_accel;
float accel_y = ay; // - base_y_accel;
float accel_z = az; // - base_z_accel;


   float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
   float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
   float accel_angle_z = 0;

   // Compute the (filtered) gyro angles
   float dt =(t_now - get_last_time())/1000.0;
   float gyro_angle_x = gyro_x*dt + get_last_x_angle();
   float gyro_angle_y = gyro_y*dt + get_last_y_angle();
   float gyro_angle_z = gyro_z*dt + get_last_z_angle();

   // Compute the drifting gyro angles
   float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle();
   float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle();
   float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();

   // Apply the complementary filter to figure out the change in angle - choice of alpha is
   // estimated now.Alpha depends on the sampling rate...
   const float alpha = 0.96;
   float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;
   float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y;
   float angle_z = gyro_angle_z;//Accelerometer doesn't give z-angle

   // Update the saved data with the latest values
   set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);

   // Output complementary data and DMP data to the serial port.The signs on the data needed to be
   // fudged to get the angle direction correct.
   Serial.print(get_last_x_angle(), 2);
   Serial.print(",");
   Serial.print(get_last_y_angle(), 2);
   Serial.print(",");
   Serial.print(-get_last_z_angle(), 2);
   Serial.print(",");
   Serial.print(ax/16384.0, 2);
   Serial.print(",");
   Serial.print(ay/16384.0, 2);
   Serial.print(",");
   Serial.println(az/16384.0, 2);


   // blink LED to indicate activity

   delay(110);

}

pijunkuan 发表于 2014-12-6 21:15:21

为什么没有人回复呢？

Tottiii 发表于 2014-12-6 22:10:51

pijunkuan 发表于 2014-12-6 21:15 static/image/common/back.gif
为什么没有人回复呢？

多特蒙德:lol

pijunkuan 发表于 2014-12-7 00:25:44

Tottiii 发表于 2014-12-6 22:10 static/image/common/back.gif
多特蒙德

恩啊，你也是吗

Tottiii 发表于 2014-12-7 00:28:14

是，可惜最近排名太差。。。惨不忍睹

pijunkuan 发表于 2014-12-7 00:39:09

Tottiii 发表于 2014-12-7 00:28 static/image/common/back.gif
是，可惜最近排名太差。。。惨不忍睹

你在多特蒙德哪啊？什么排名太差了啊

页: [1]

极客工坊's Archiver

Arduino+MPU 6050 学习记录