Arduino 与 MPU6050 姿态解算+ PROCESSING

买的MPU6050自带姿态解算大大减轻了上层处理器所做的工作。 通过熟悉了一下processing之后做了一个小例子更是感觉这个传感器的奇妙。

Arduino部分

主要是读取MPU6050数据并将采集到的欧拉角通过串口打印到上位机，采集数据很简单，MPU6050接到arduino mega2560的serial1上便可接收数据，然后通过serial传输到电脑上。MPU6050自带了卡尔曼滤波，所以上层更是直接使用了数据，观测得误差确实很少，很实用的传感器。

注: MPU6050使用的是串口模式，配置时掉电保存的，波特率为9600

#include <Wire.h>
byte A[11];

void setup()
{
	Serial.begin(115200);
	Serial1.begin(9600);
	Serial.println("initial ...");
	Serial1.write(0xff);
	Serial1.write(0xaa);
	Serial1.write(0x52);
	Serial1.flush();
	Serial.println("end");
}

static int counter=0;
char buffer[20];
void loop()
{
	if(counter==0&&A[0]==0x55)
	{
		int index=2;
		switch(A[1])
		{
			case 0x53:
			{

				float xa=(A[index]|(A[index+1]<<8))/32768.0*180;
				float ya=(A[index+2]|(A[index+3]<<8))/32768.0*180;
				float za=(A[index+4]|(A[index+5]<<8))/32768.0*180;
				//if(abs(temp-97.77)<3) break;
				//sprintf(buffer,"%lf,",xa);
				Serial.print("x=");
				Serial.println(xa);
				//sprintf(buffer,"%lf,",ya);
				Serial.print("y=");
				Serial.println(ya);
				//sprintf(buffer,"%lf",za);
				Serial.print("z=");
				Serial.println(za);
				A[0]=0;
				delay(50);
			}
				break;
			default:
			break;
		}
	}
}

void serialEvent1()
{
	while(Serial1.available())
	{
		A[counter]=Serial1.read();
		if(counter==0&&A[counter]!=0x55) return;
		counter=(counter+1)%11;
	}
}
void serialEvent()
{
	char buf[255];
	int i=0;
	while(Serial.available()) buf[i++]=Serial.read();
	buf[i]=0;
	if(strcmp("stop",buf)==0) Serial1.end();
	else if(strcmp("run",buf)==0) Serial1.begin(115200);
}

MPU6050的四根线连到arduino后便可源源不断地接收到数据，上传到电脑上的格式为:
 x=12.44

y=56.01

z=67.34

下面的processing程序会根据这个来解析收到的数据并且调整姿态。

Processing部分

processing这个程序昨天第一次听说，看到国外视频里有人做的模型看着实在舒服，所以也想尝试一下。虽然官网上称Processing为一门语言，不过个人感觉语法就是java+opengl+p3d+c++混合而成。传送门

通过arduino串口发送出来的消息，在processing接收处理，没有太多可说的，大致感觉下

processing

import processing.serial.*;
import processing.opengl.*;

Serial myserial;
float value[]=new float[4];
String rf[]=new String[4];
PGraphics graphics1;
PGraphics graphics2;
PGraphics graphics3;
PGraphics graphics4;
void setup()
{
  size(600,600,P2D);
  String name="COM5";
  myserial=new Serial(this,name,115200);
  smooth();
  for(int i=0;i<4;++i) value[i]=0;

   graphics1=createGraphics(298,298,OPENGL);
   graphics2=createGraphics(298,298,OPENGL);
   graphics3=createGraphics(298,298,OPENGL);
   graphics4=createGraphics(298,298,OPENGL);
   textMode(MODEL);
   textSize(32);
}
long lasttime=0;

void draw()
{
  frameRate(60);
  drawRect(graphics1,0,0,1,0); //FONT
  drawRect(graphics2,90,0,1,0);//lLEFT
  drawRect(graphics3,-90,1,0,0);//TOP
  drawRect(graphics4,90,1,1,0);//RANDOM

  image(graphics1,0,0);
  image(graphics2,300,0);
  image(graphics3,0,300);
  image(graphics4,300,300);
  fill(248,147,147);
  text("FONT VIEW",10,50);
  text("LEFT VIEW",310,50);
  text("TOP VIEW",10,350);
  text("RANDOM VIEW",310,350);
  lasttime=millis();
}

void drawRect(PGraphics pg,float rot,int x,int y,int z)
{
  pg.beginDraw();
  pg.lights();
  pg.background(126);
  pg.textSize(20);
  pg.fill(3,60,244);
  long framerate=1000/(millis()-lasttime);
  pg.text("fps:"+framerate,200,20);

  pg.fill(246,225,65);
  pg.translate(100,100,-100);
  pg.rotate(rot*PI/180,x,y,z);
  pg.rotateY(value[2]*PI/180);
  pg.rotateZ(-value[1]*PI/180);
  pg.rotateX(value[0]*PI/180);
  pg.box(50,50,100);
  pg.endDraw();
}
String st;
void serialEvent(Serial p)
{
    st=p.readStringUntil(10);
    if(st==null) return;
    if(st.indexOf("x=")!=-1)
    {
      value[0]=float(st.substring(2));
    }
    else if(st.indexOf("y=")!=-1)
    {
      value[1]=float(st.substring(2));
    }
    else if(st.indexOf("z=")!=-1)
    {
      value[2]=float(st.substring(2));
    }
}

另外做了一个演示视频 click