首页 › Arduino › 求助帖：MPU6050 姿态识别

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

求助帖：MPU6050 姿态识别

http://v.qq.com/x/page/n0321kt2vqz.html

Arduino程序：


#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;
Quaternion q;
VectorFloat g;
float YawPitchRoll;

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);
      Serial.print(',');
      Serial.print(YawPitchRoll);
      Serial.print(',');
      Serial.print(YawPitchRoll);
      Serial.println();
      delay(300);
    }
    mpu.resetFIFO();
}




Processing程序：
import processing.serial.*;

Serial myPort;// 创建串口对象myPort
PFont Font;
boolean firstSample = true;
float [] YawPitchRoll = new float;      
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);
rotateY(-YawPitchRoll);
rotateZ(YawPitchRoll);
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*180/PI) +"°", 20, 50);
text("Pitch:" + df.format(YawPitchRoll*180/PI)+"°", 20, 150);
text("Roll :" + df.format(YawPitchRoll*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 < 0){
   a = -sin(YawPitchRoll);
   b = cos(-YawPitchRoll);
}
else{
   a = sin(YawPitchRoll);
   b = cos(YawPitchRoll);
}
if (YawPitchRoll < 0){
   c = -sin(YawPitchRoll);
   d = cos(-YawPitchRoll);
}
else{
   c = sin(YawPitchRoll);
   d = cos(YawPitchRoll);   
}

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.charAt(0) == 'h'){
      println("device is working");
      YawPitchRoll = Float.parseFloat(str);
      YawPitchRoll = Float.parseFloat(str);
      YawPitchRoll = Float.parseFloat(str);
      }
    }
}


问题：没有移动MPU6050,但是YAW角度一直有变化，请教各位大神 这是什么原因？ 如何解决？
视频：
http://v.qq.com/x/page/r03218r14yg.html

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

Arduinoc程序：
#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;
Quaternion q;
VectorFloat g;
float YawPitchRoll;

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);
      Serial.print(',');
      Serial.print(YawPitchRoll);
      Serial.print(',');
      Serial.print(YawPitchRoll);
      Serial.println();
      delay(300);
    }
    mpu.resetFIFO();
}



致谢：
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=4867&highlight=mpu6050
http://www.geek-workshop.com/forum.php?mod=viewthread&tid=15392&extra=&highlight=mpu6050&page=1
http://www.geek-workshop.com/thread-1935-1-1.html

查看完整版本: 求助帖：MPU6050 姿态识别