求助帖：MPU6050 姿态识别

柱头ANDY · 发表于 2016-8-18 15:49:56

Arduino程序：

[code]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat g;
float YawPitchRoll[3];
void setup() {
Wire.begin();
Serial.begin(9600);
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
}
void loop() {
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024){
mpu.resetFIFO();
}
else {
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer,packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q , fifoBuffer);
mpu.dmpGetGravity(&g , &q);
mpu.dmpGetYawPitchRoll(YawPitchRoll , &q , &g);
Serial.print('h');
Serial.print(',');
Serial.print(YawPitchRoll[0]);
Serial.print(',');
Serial.print(YawPitchRoll[1]);
Serial.print(',');
Serial.print(YawPitchRoll[2]);
Serial.println();
delay(300);
}
mpu.resetFIFO();
}

复制代码

[/code]

Processing程序：
[pre lang="Processing" line="1"]import processing.serial.*;

Serial myPort;  // 创建串口对象myPort
PFont Font;
boolean firstSample = true;
float [] YawPitchRoll = new float[3];
java.text.DecimalFormat df =new java.text.DecimalFormat("#.00");
void setup()
{
  size(600, 600, P3D);
  myPort = new Serial(this, "COM4", 9600);
  Font = loadFont("David-30.vlw");
  textFont(Font);
  myPort.bufferUntil('\n');
}

void buildBoxShape() {


  beginShape(QUADS);
  fill(#CDB38B);
  vertex(-20, -5, 30);
  vertex(20, -5, 30);
  vertex(20, 5, 30);
  vertex(-20, 5, 30);

  //Z-
  fill(#8B795E);
  vertex(-20, -5, -30);
  vertex(20, -5, -30);
  vertex(20, 5, -30);
  vertex(-20, 5, -30);

  //X-
  fill(#FFFACD);
  vertex(-20, -5, -30);
  vertex(-20, -5, 30);
  vertex(-20, 5, 30);
  vertex(-20, 5, -30);

  //X+
  fill(#EEE9BF);
  vertex(20, -5, -30);
  vertex(20, -5, 30);
  vertex(20, 5, 30);
  vertex(20, 5, -30);

  //Y-
  fill(#CDC9A5);
  vertex(-20, -5, -30);
  vertex(20, -5, -30);
  vertex(20, -5, 30);
  vertex(-20, -5, 30);

  //Y+
  fill(#8B8970);
  vertex(-20, 5, -30);
  vertex(20, 5, -30);
  vertex(20, 5, 30);
  vertex(-20, 5, 30);

  endShape();

}

void drawCube() {
  pushMatrix();
  translate(300, 450, 0);
  scale(4, 4, 4);

  rotateX(YawPitchRoll[2]);
  rotateY(-YawPitchRoll[0]);
  rotateZ(YawPitchRoll[1]);
  buildBoxShape();

  popMatrix();
}

void draw() {
  float a,b,c,d;
  background(#ffffff);
  fill(#000000);

  //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  text("Yaw :" + df.format(YawPitchRoll[0]*180/PI) +"°", 20, 50);
  text("Pitch:" + df.format(YawPitchRoll[2]*180/PI)+"°", 20, 150);
  text("Roll :" + df.format(YawPitchRoll[1]*180/PI) +"°", 20, 250);

//display axes显示轴
  pushMatrix();
  translate(450, 150, 0);
  stroke(#008B8B);
  line(0, 0, 0, 100, 0, 0);
  stroke(#8B008B);
  line(0, 0, 0, 0, 100, 0);
  stroke(#8B0000);
  line(0, 0, 0, 0, 0, 100);
  if (YawPitchRoll[2] < 0){
   a = -sin(YawPitchRoll[2]);
   b = cos(-YawPitchRoll[2]);
  }
  else{
   a = sin(YawPitchRoll[2]);
   b = cos(YawPitchRoll[2]);
  }
  if (YawPitchRoll[0] < 0){
   c = -sin(YawPitchRoll[0]);
   d = cos(-YawPitchRoll[0]);
  }
  else{
   c = sin(YawPitchRoll[0]);
   d = cos(YawPitchRoll[0]);
  }

  line(0, 0, 0,100*b*d , 100*b*c, 100*a);
  popMatrix();
  stroke(#FFFFFF);
  drawCube();
}

void serialEvent(Serial p){
String [] str;
String inString = p.readString();
if (inString != null){
   str = inString.split(",");
   if (str[0].charAt(0) == 'h'){
      println("device is working");
      YawPitchRoll[0] = Float.parseFloat(str[1]);
      YawPitchRoll[1] = Float.parseFloat(str[2]);
      YawPitchRoll[2] = Float.parseFloat(str[3]);
   }
}
}[/code]

问题：没有移动MPU6050,但是YAW角度一直有变化，请教各位大神这是什么原因？如何解决？
视频：

柱头ANDY · 发表于 2016-8-18 15:53:00

Arduinoc程序：

[code]
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat g;
float YawPitchRoll[3];
void setup() {
Wire.begin();
Serial.begin(9600);
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setDMPEnabled(true);
packetSize = mpu.dmpGetFIFOPacketSize();
}
void loop() {
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024){
mpu.resetFIFO();
}
else {
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer,packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q , fifoBuffer);
mpu.dmpGetGravity(&g , &q);
mpu.dmpGetYawPitchRoll(YawPitchRoll , &q , &g);
Serial.print('h');
Serial.print(',');
Serial.print(YawPitchRoll[0]);
Serial.print(',');
Serial.print(YawPitchRoll[1]);
Serial.print(',');
Serial.print(YawPitchRoll[2]);
Serial.println();
delay(300);
}
mpu.resetFIFO();
}

复制代码

[/code]

致谢：
http://www.geek-workshop.com/for ... p;highlight=mpu6050
http://www.geek-workshop.com/for ... =mpu6050&page=1
http://www.geek-workshop.com/thread-1935-1-1.html

帐号		自动登录	找回密码
密码			注册