MPU6050姿态识别【抛砖】，引'弘毅''Randy'等人【抛玉】
本帖最后由 Ansifa 于 2013-3-4 01:51 编辑Ansifa注：下面的程序我打包了一个压缩包，已经验证编译。大家可以直接下载
2012年10月26日更新：
把转换原始数据地方改成：
RwAcc = float(inputStringArr) / 16384.0;
RwAcc = float(inputStringArr)/ 16384.0;
RwAcc = float(inputStringArr)/ 16384.0;
// 把原始数据转换为"度/秒"
Gyro = float(inputStringArr) / 131.0;
Gyro = float(inputStringArr) / 131.0;
Gyro = float(inputStringArr) /131.0;
其实Randy的帖子（获取陀螺仪与加速度相关数据的Arduino示例代码 ）里面demo程序已经是这个参数了，但因为没使用浮点形式（即必需使用16384.0，而非16384）导致最后ax/ay/az的值都为0了，所以也就反映不出加速度的微小变化了。
在更新之前，当把模块树立时，processing里显示的3D方块会慢慢恢复到水平状态；而通过上述更新之后，再把模块树立，processing里显示的3D方块也会保持树立状态，这才是正确的！
======================================================
从 Randy 他们公司购买的 MPU6050 模块，集成陀螺仪与加速度传感器。虽然 Randy 已经提供了获取陀螺仪与加速度相关数据的Arduino示例代码 ，但怎么用这些数据来展现当前的姿态却也还是个问题；后来看到弘毅关于ITG3205与ADXL345的姿态识别帖子，然后抄过来一试，效果还真的出来了。
Arduino代码：
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
char str;
void setup() {
Wire.begin();
Serial.begin(9600);
accelgyro.initialize();
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sprintf(str, "%d,%d,%d,%d,%d,%d,%d", ax, ay, az, gx, gy, gz);
Serial.print(str);
Serial.write(byte(10));
delay(50);
}
Processing代码：
import processing.serial.*;
Serial myPort;// 创建串口对象myPort
boolean firstSample = true;
float [] RwAcc = new float; // 通过加速度传感器把重力加速度投影在x/y/z三轴上
float [] Gyro = new float; // 陀螺仪读取
float [] RwGyro = new float; // 重新读取陀螺仪
float [] Awz = new float; // XZ/ YZ平面和Z轴（度）R的投影之间的角度
float [] RwEst = new float;
int lastTime = 0;
int interval = 0;
float wGyro = 10.0;
int lf = 10; // 10在ASCII表中表示'\n'
byte[] inBuffer = new byte;
PFont font;
final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
//myPort = new Serial(this, Serial.list(), 9600); // 设置电脑第三个COM口为连接端口，这个要根据你电脑情况进行设置。
myPort = new Serial(this, "/dev/ttyUSB0", 9600);
// 加载字体，字体必须在代码文件同目录下的data文件夹中
font = loadFont("/home/zwang/processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters/data/CourierNew36.vlw");
}
void readSensors() {
if (myPort.available() > 0) {
if (myPort.readBytesUntil(lf, inBuffer) > 0) {
String inputString = new String(inBuffer);
String [] inputStringArr = split(inputString, ',');
// 把原始数据转换为G
RwAcc = float(inputStringArr) / 16384.0;
RwAcc = float(inputStringArr)/ 16384.0;
RwAcc = float(inputStringArr)/ 16384.0;
// 把原始数据转换为"度/秒"
Gyro = float(inputStringArr) / 131.0;
Gyro = float(inputStringArr) / 131.0;
Gyro = float(inputStringArr) / 131.0;
}
}
}
void normalize3DVec(float [] vector) {
float R;
R = sqrt(vector*vector + vector*vector + vector*vector);
vector /= R;
vector /= R;
vector /= R;
}
float squared(float x) {
return x*x;
}
void buildBoxShape() {
//box(60, 10, 40);
noStroke();
beginShape(QUADS);
//Z+ (绘图区域)
fill(#00ff00);
vertex(-30, -5, 20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
//Z-
fill(#0000ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, 5, -20);
vertex(-30, 5, -20);
//X-
fill(#ff0000);
vertex(-30, -5, -20);
vertex(-30, -5, 20);
vertex(-30, 5, 20);
vertex(-30, 5, -20);
//X+
fill(#ffff00);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(30, 5, -20);
//Y-
fill(#ff00ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(-30, -5, 20);
//Y+
fill(#00ffff);
vertex(-30, 5, -20);
vertex(30, 5, -20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
endShape();
}
void drawCube() {
pushMatrix();
translate(300, 450, 0);
scale(4, 4, 4);
rotateX(HALF_PI * -RwEst);
rotateZ(HALF_PI * RwEst);
buildBoxShape();
popMatrix();
}
void getInclination() {
int w = 0;
float tmpf = 0.0;
int currentTime, signRzGyro;
readSensors();
normalize3DVec(RwAcc);
currentTime = millis();
interval = currentTime - lastTime;
lastTime = currentTime;
if (firstSample || Float.isNaN(RwEst)) { // NaN用来等待检查从arduino过来的数据
for (w=0;w<=2;w++) {
RwEst = RwAcc; // 初始化加速度传感器读数
}
}
else {
// 对RwGyro进行评估
if (abs(RwEst) < 0.1) {
// Rz值非常的小，它的作用是作为Axz与Ayz的计算参照值，防止放大的波动产生错误的结果。
// 这种情况下就跳过当前的陀螺仪数据，使用以前的。
for (w=0;w<=2;w++) {
RwGyro = RwEst;
}
}
else {
// ZX/ZY平面和Z轴R的投影之间的角度，基于最近一次的RwEst值
for (w=0;w<=1;w++) {
tmpf = Gyro; // 获取当前陀螺仪的deg/s
tmpf *= interval / 1000.0f; // 得到角度变化值
Awz = atan2(RwEst, RwEst) * 180 / PI; // 得到角度并转换为度
Awz += tmpf; // 根据陀螺仪的运动得到更新后的角度
}
// 判断RzGyro是多少，主要看Axz的弧度是多少
// 当Axz在-90 ..90 => cos(Awz) >= 0这个范围内的时候RzGyro是准确的
signRzGyro = ( cos(Awz * PI / 180) >=0 ) ? 1 : -1;
// 从Awz的角度值反向计算RwGyro的公式请查看网页 http://starlino.com/imu_guide.html
for (w=0;w<=1;w++) {
RwGyro = sin(Awz * PI / 180);
RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
RwGyro = sin(Awz * PI / 180);
RwGyro /= sqrt( 1 + squared(cos(Awz * PI / 180)) * squared(tan(Awz * PI / 180)) );
}
RwGyro = signRzGyro * sqrt(1 - squared(RwGyro) - squared(RwGyro));
}
// 把陀螺仪与加速度传感器的值进行结合
for (w=0;w<=2;w++) RwEst = (RwAcc + wGyro * RwGyro) / (1 + wGyro);
normalize3DVec(RwEst);
}
firstSample = false;
}
void draw() {
getInclination();
background(#000000);
fill(#ffffff);
textFont(font, 20);
//float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
//text("temp:\n" + temp_decoded + " C", 350, 250);
text("RwAcc (G):\n" + RwAcc + "\n" + RwAcc + "\n" + RwAcc + "\ninterval: " + interval, 20, 50);
text("Gyro (°/s):\n" + Gyro + "\n" + Gyro + "\n" + Gyro, 220, 50);
text("Awz (°):\n" + Awz + "\n" + Awz, 420, 50);
text("RwGyro (°/s):\n" + RwGyro + "\n" + RwGyro + "\n" + RwGyro, 20, 180);
text("RwEst :\n" + RwEst + "\n" + RwEst + "\n" + RwEst, 220, 180);
// display axes显示轴
pushMatrix();
translate(450, 250, 0);
stroke(#ffffff);
scale(100, 100, 100);
line(0, 0, 0, 1, 0, 0);
line(0, 0, 0, 0, -1, 0);
line(0, 0, 0, 0, 0, 1);
line(0, 0, 0, -RwEst, RwEst, RwEst);
popMatrix();
drawCube();
}
注意：需要更改上面的 myPort和 font 的定义。
然后运行Processing就可以看到效果了。
http://player.youku.com/player.php/sid/XNDUxNDYxMTQ4/v.swf 因为没怎么修改过，显示结果貌似还有些误差：如水平放置模块的时候，3D方块稍有点角度；
还有就是 弘毅 版主 这两天可能会放出一个基于MPU6050姿态识别的改进版本，期待之中。。。 {:soso_e154:}压力山大。。。我得去努力。。。 不错，与processing 完美全体哇，:D 看代码发现，LZ，用的UBUNTU哇，牛鸡哇，再支持一下 好东西啊，这个模块很奇怪，一开始用你会觉得和坏的没两样，需要注意的细节也非常多。你的模块能用，恭喜！！！ 学习了，原来还可以这样玩{:soso_e113:} Processing不了解啊，有LABVIEW的不？？？？ 本帖最后由 wzc0066 于 2012-9-18 10:00 编辑
太行摄狼 发表于 2012-9-18 08:56 static/image/common/back.gif
Processing不了解啊，有LABVIEW的不？？？？
我也刚接触Processing，Arduino的IDE就是从Processing基础上修改而来的。所以使用过Arduino的可能会感觉比较熟悉。
至于语法方面，感觉就是将一些Java的操作（如果绘制各种图形等）再进行一次封装（？？），所以调用更简洁。
总得来说，入手比较简单，可以一试。 这个字体代码错误？？哪里下载的？{:soso_e132:} 菩提明镜 发表于 2012-9-18 11:54 static/image/common/back.gif
这个字体代码错误？？哪里下载的？
就在processing目录下：processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters/data/CourierNew36.vlw
那是我系统的绝对路径，你要改成你的绝对路径。
抱歉，不是说MPU6050出来的已经是数字的了吗？ 如下的转换 能解释下是啥意思吗？
// 把原始数据转换为G44.
RwAcc = float(inputStringArr) / 256.0;45.
RwAcc = float(inputStringArr)/ 256.0;46.
RwAcc = float(inputStringArr)/ 256.0;47. 48.
// 把原始数据转换为"度/秒"49.
Gyro = float(inputStringArr) / 14.375;50.
Gyro = float(inputStringArr) / 14.375;51.
Gyro = float(inputStringArr) / 14.375;
本帖最后由 wzc0066 于 2012-9-18 14:53 编辑
详细参见弘毅版主的帖子：关于ITG3205与ADXL345的姿态识别
Processing程序是抄他的，去他的帖子问下请他给你解释一下。 wzc0066 发表于 2012-9-18 12:06 static/image/common/back.gif
就在processing目录下：processing/processing-1.5.1/modes/android/examples/Basics/Typography/Letters ...
搞定了 谢谢了哦:lol 学习了:lol