创客：在Arduino UNO 上使用MUP6050加速度计

本文目录

接线图

MPU6050 与 Arduino UNO 之间的连线图：

实物图：

演示视频：

Arduino代码，用来获取加速度计的各项数据

这里需要安装MPU6050的库文件

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include
const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}
void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print(AcX);
  Serial.print(","); 
  Serial.print(AcY);
  Serial.print(","); 
  Serial.print(AcZ);
  Serial.print(","); 
  Serial.print(Tmp/340.00 36.53);  //equation for temperature in degrees C from datasheet
  Serial.print(","); 
  Serial.print(GyX);
  Serial.print(","); 
  Serial.print(GyY);
  Serial.print(","); 
  Serial.println(GyZ);
  delay(50);
}

// MPU-6050 Short Example Sketch

// By Arduino User JohnChi

// August 17, 2014

// Public Domain

#include

const int MPU_addr=0x68; // I2C address of the MPU-6050

int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup(){

Wire.begin();

Wire.beginTransmission(MPU_addr);

Wire.write(0x6B); // PWR_MGMT_1 register

Wire.write(0); // set to zero (wakes up the MPU-6050)

Wire.endTransmission(true);

Serial.begin(9600);

}

void loop(){

Wire.beginTransmission(MPU_addr);

Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)

Wire.endTransmission(false);

Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers

AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)

AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)

AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)

GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)

GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

Serial.print(AcX);

Serial.print(",");

Serial.print(AcY);

Serial.print(",");

Serial.print(AcZ);

Serial.print(",");

Serial.print(Tmp/340.00 36.53); //equation for temperature in degrees C from datasheet

Serial.print(",");

Serial.print(GyX);

Serial.print(",");

Serial.print(GyY);

Serial.print(",");

Serial.println(GyZ);

delay(50);

}

Python代码，可以3D展示加速度计的姿态。如果不想用Python，还可以用Processing，之后有Processing的代码。

需要安装额外的运行环境。

"""
    成功运行
"""

#!/usr/bin/python

import pygame
from OpenGL.GL import *
from OpenGL.GLU import *
from math import radians
from pygame.locals import *

# 导入通信的模块
import serial

# 创建串口通信，串口号要换成自己的
ser = serial.Serial('COM17', 9600, timeout=0.5)


SCREEN_SIZE = (1024, 768)
SCALAR = .5
SCALAR2 = 0.2


def resize(width, height):
    glViewport(0, 0, width, height)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(45.0, float(width) / height, 0.001, 10.0)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    gluLookAt(0.0, 1.0, -5.0,
              0.0, 0.0, 0.0,
              0.0, 1.0, 0.0)


def init():
    glEnable(GL_DEPTH_TEST)
    glClearColor(0.0, 0.0, 0.0, 0.0)
    glShadeModel(GL_SMOOTH)
    glEnable(GL_BLEND)
    glEnable(GL_POLYGON_SMOOTH)
    glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
    glEnable(GL_COLOR_MATERIAL)
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));


def read_values():
    # 读取串口的信息
    val = ser.readline().decode('utf-8')

    # 以逗号来分割传入的数据
    parsed = val.split(',')

    # 去除末尾的数据
    parsed = [x.rstrip() for x in parsed]

    if(len(parsed) > 2):
        # 把数据转化为数字类型
        parsed = [eval(x) for x in parsed]
        return parsed
    else:
        return [0,0,0,0,0]


def run():
    pygame.init()
    screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
    resize(*SCREEN_SIZE)
    init()
    clock = pygame.time.Clock()
    cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7))
    angle = 0

    while True:
        then = pygame.time.get_ticks()
        for event in pygame.event.get():
            if event.type == QUIT:
                return
            if event.type == KEYUP and event.key == K_ESCAPE:
                return

        values = read_values()
        x_angle = values[0]/ 131
        print(x_angle)
        y_angle = values[1]/ 131
        print(y_angle)

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

        glColor((1., 1., 1.))
        glLineWidth(1)
        glBegin(GL_LINES)

        for x in range(-20, 22, 2):
            glVertex3f(x / 10., -1, -1)
            glVertex3f(x / 10., -1, 1)

        for x in range(-20, 22, 2):
            glVertex3f(x / 10., -1, 1)
            glVertex3f(x / 10., 1, 1)

        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z / 10.)
            glVertex3f(2, -1, z / 10.)

        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z / 10.)
            glVertex3f(-2, 1, z / 10.)

        for z in range(-10, 12, 2):
            glVertex3f(2, -1, z / 10.)
            glVertex3f(2, 1, z / 10.)

        for y in range(-10, 12, 2):
            glVertex3f(-2, y / 10., 1)
            glVertex3f(2, y / 10., 1)

        for y in range(-10, 12, 2):
            glVertex3f(-2, y / 10., 1)
            glVertex3f(-2, y / 10., -1)

        for y in range(-10, 12, 2):
            glVertex3f(2, y / 10., 1)
            glVertex3f(2, y / 10., -1)

        glEnd()
        glPushMatrix()
        glRotate(float(x_angle), 1, 0, 0)
        glRotate(-float(y_angle), 0, 0, 1)
        cube.render()
        glPopMatrix()
        pygame.display.flip()


class Cube(object):

    def __init__(self, position, color):
        self.position = position
        self.color = color

    # Cube information
    num_faces = 6

    vertices = [(-1.0, -0.05, 0.5),
                (1.0, -0.05, 0.5),
                (1.0, 0.05, 0.5),
                (-1.0, 0.05, 0.5),
                (-1.0, -0.05, -0.5),
                (1.0, -0.05, -0.5),
                (1.0, 0.05, -0.5),
                (-1.0, 0.05, -0.5)]

    normals = [(0.0, 0.0,  1.0),  # front
               (0.0, 0.0, -1.0),  # back
               ( 1.0, 0.0, 0.0),  # right
               (-1.0, 0.0, 0.0),  # left
               (0.0,  1.0, 0.0),  # top
               (0.0, -1.0, 0.0)]  # bottom

    vertex_indices = [(0, 1, 2, 3),  # front
                      (4, 5, 6, 7),  # back
                      (1, 5, 6, 2),  # right
                      (0, 4, 7, 3),  # left
                      (3, 2, 6, 7),  # top
                      (0, 1, 5, 4)]  # bottom

    def render(self):
        then = pygame.time.get_ticks()
        glColor(self.color)

        vertices = self.vertices

        # Draw all 6 faces of the cube
        glBegin(GL_QUADS)

        for face_no in  range(self.num_faces):
            glNormal3dv(self.normals[face_no])
            v1, v2, v3, v4 = self.vertex_indices[face_no]
            glVertex(vertices[v1])
            glVertex(vertices[v2])
            glVertex(vertices[v3])
            glVertex(vertices[v4])
        glEnd()


if __name__ == "__main__":
    run()

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

"""

成功运行

"""

#!/usr/bin/python

import pygame

from OpenGL.GL import *

from OpenGL.GLU import *

from math import radians

from pygame.locals import *

# 导入通信的模块

import serial

# 创建串口通信，串口号要换成自己的

ser = serial.Serial('COM17', 9600, timeout=0.5)

SCREEN_SIZE = (1024, 768)

SCALAR = .5

SCALAR2 = 0.2

def resize(width, height):

glViewport(0, 0, width, height)

glMatrixMode(GL_PROJECTION)

glLoadIdentity()

gluPerspective(45.0, float(width) / height, 0.001, 10.0)

glMatrixMode(GL_MODELVIEW)

glLoadIdentity()

gluLookAt(0.0, 1.0, -5.0,

0.0, 0.0, 0.0,

0.0, 1.0, 0.0)

def init():

glEnable(GL_DEPTH_TEST)

glClearColor(0.0, 0.0, 0.0, 0.0)

glShadeModel(GL_SMOOTH)

glEnable(GL_BLEND)

glEnable(GL_POLYGON_SMOOTH)

glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)

glEnable(GL_COLOR_MATERIAL)

glEnable(GL_LIGHTING)

glEnable(GL_LIGHT0)

glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));

def read_values():

# 读取串口的信息

val = ser.readline().decode('utf-8')

# 以逗号来分割传入的数据

parsed = val.split(',')

# 去除末尾的数据

parsed = [x.rstrip() for x in parsed]

if(len(parsed) > 2):

# 把数据转化为数字类型

parsed = [eval(x) for x in parsed]

return parsed

else:

return [0,0,0,0,0]

def run():

pygame.init()

screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)

resize(*SCREEN_SIZE)

init()

clock = pygame.time.Clock()

cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7))

angle = 0

while True:

then = pygame.time.get_ticks()

for event in pygame.event.get():

if event.type == QUIT:

return

if event.type == KEYUP and event.key == K_ESCAPE:

return

values = read_values()

x_angle = values[0]/ 131

print(x_angle)

y_angle = values[1]/ 131

print(y_angle)

glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

glColor((1., 1., 1.))

glLineWidth(1)

glBegin(GL_LINES)

for x in range(-20, 22, 2):

glVertex3f(x / 10., -1, -1)

glVertex3f(x / 10., -1, 1)

for x in range(-20, 22, 2):

glVertex3f(x / 10., -1, 1)

glVertex3f(x / 10., 1, 1)

for z in range(-10, 12, 2):

glVertex3f(-2, -1, z / 10.)

glVertex3f(2, -1, z / 10.)

for z in range(-10, 12, 2):

glVertex3f(-2, -1, z / 10.)

glVertex3f(-2, 1, z / 10.)

for z in range(-10, 12, 2):

glVertex3f(2, -1, z / 10.)

glVertex3f(2, 1, z / 10.)

for y in range(-10, 12, 2):

glVertex3f(-2, y / 10., 1)

glVertex3f(2, y / 10., 1)

for y in range(-10, 12, 2):

glVertex3f(-2, y / 10., 1)

glVertex3f(-2, y / 10., -1)

for y in range(-10, 12, 2):

glVertex3f(2, y / 10., 1)

glVertex3f(2, y / 10., -1)

glEnd()

glPushMatrix()

glRotate(float(x_angle), 1, 0, 0)

glRotate(-float(y_angle), 0, 0, 1)

cube.render()

glPopMatrix()

pygame.display.flip()

class Cube(object):

def __init__(self, position, color):

self.position = position

self.color = color

# Cube information

num_faces = 6

vertices = [(-1.0, -0.05, 0.5),

(1.0, -0.05, 0.5),

(1.0, 0.05, 0.5),

(-1.0, 0.05, 0.5),

(-1.0, -0.05, -0.5),

(1.0, -0.05, -0.5),

(1.0, 0.05, -0.5),

(-1.0, 0.05, -0.5)]

normals = [(0.0, 0.0, 1.0), # front

(0.0, 0.0, -1.0), # back

( 1.0, 0.0, 0.0), # right

(-1.0, 0.0, 0.0), # left

(0.0, 1.0, 0.0), # top

(0.0, -1.0, 0.0)] # bottom

vertex_indices = [(0, 1, 2, 3), # front

(4, 5, 6, 7), # back

(1, 5, 6, 2), # right

(0, 4, 7, 3), # left

(3, 2, 6, 7), # top

(0, 1, 5, 4)] # bottom

def render(self):

then = pygame.time.get_ticks()

glColor(self.color)

vertices = self.vertices

# Draw all 6 faces of the cube

glBegin(GL_QUADS)

for face_no in range(self.num_faces):

glNormal3dv(self.normals[face_no])

v1, v2, v3, v4 = self.vertex_indices[face_no]

glVertex(vertices[v1])

glVertex(vertices[v2])

glVertex(vertices[v3])

glVertex(vertices[v4])

glEnd()

if __name__ == "__main__":

run()

Processing代码和模型文件

需要安装库文件。

// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output
// 6/20/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//     2012-06-20 - initial release

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*;

// NOTE: requires ToxicLibs to be installed in order to run properly.
// 1. Download from http://toxiclibs.org/downloads
// 2. Extract into [userdir]/Processing/libraries
//    (location may be different on Mac/Linux)
// 3. Run and bask in awesomeness

ToxiclibsSupport gfx;

Serial port;                         // The serial port
char[] teapotPacket = new char[14];  // InvenSense Teapot packet
int serialCount = 0;                 // current packet byte position
int synced = 0;
int interval = 0;

float[] q = new float[4];
Quaternion quat = new Quaternion(1, 0, 0, 0);

float[] gravity = new float[3];
float[] euler = new float[3];
float[] ypr = new float[3];

PShape s;
String message = " Arduino Uno and GY-521";

  PFont f;
  // The radius of a circle
  float r = 200;

void setup() {
    // 300px square viewport using OpenGL rendering
    size(800, 600, P3D);
    gfx = new ToxiclibsSupport(this);
    
    //**********
    s = loadShape("f15.obj");
    s.scale(14, 14, 14);
    s.rotateX(0);
    s.rotateY(0);
    s.rotateZ(3.13);
    
    
    f = createFont("Georgia",40,true);
  textFont(f);
  // The text must be centered!
  textAlign(CENTER);

    // setup lights and antialiasing
    lights();
    smooth(2);
  
    // display serial port list for debugging/clarity
    println(Serial.list());

    // get the first available port (use EITHER this OR the specific port code below)
    //String portName = Serial.list()[0];
    
    // get a specific serial port (use EITHER this OR the first-available code above)
    String portName = "COM3";
    
    // open the serial port
    port = new Serial(this, portName, 115200);
    
    // send single character to trigger DMP init/start
    // (expected by MPU6050_DMP6 example Arduino sketch)
    port.write('r');
}

void draw() {
    if (millis() - interval > 1000) {
        // resend single character to trigger DMP init/start
        // in case the MPU is halted/reset while applet is running
        port.write('r');
        interval = millis();
    }
    
    // black background
    background(5,120,190);
    
    // translate everything to the middle of the viewport
    pushMatrix();
    translate(width / 2, height / 2);

    // 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
    // ...and other weirdness I haven't figured out yet
    //rotateY(-ypr[0]);
    //rotateZ(-ypr[1]);
    //rotateX(-ypr[2]);

    // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
    // (axis order [1, 3, 2] and inversion [-1,  1,  1] is a consequence of
    // different coordinate system orientation assumptions between Processing
    // and InvenSense DMP)
    float[] axis = quat.toAxisAngle();
    rotate(axis[0], -axis[1], axis[3], axis[2]);

    noStroke();
    directionalLight(185, 200, 255, 0, 1, -1);
    ambientLight(120, 125, 185, 35, 24, 90);
    shape(s, 0, 0);

    popMatrix();
        
    // Start in the center and draw the circle
    translate(width / 2, height / 2);
    noFill();
    stroke(0);

  // We must keep track of our position along the curve
  float arclength = 0;

  // For every box
  for (int i = 0; i < message.length(); i  )
  {
    // Instead of a constant width, we check the width of each character.
    char currentChar = message.charAt(i);
    float w = textWidth(currentChar);

    // Each box is centered so we move half the width
    arclength  = w/1.5;
    // Angle in radians is the arclength divided by the radius
    // Starting on the left side of the circle by adding PI
    float theta = PI   arclength / r;    

    pushMatrix();
    // Polar to cartesian coordinate conversion
    translate(r*cos(theta), r*sin(theta));
    // Rotate the box
    rotate(theta PI/2); // rotation is offset by 90 degrees
    // Display the character
    fill(0);
    text(currentChar,0,0);
    popMatrix();
    // Move halfway again
    arclength  = w/1.5;
  }
   
}

void serialEvent(Serial port) {
    interval = millis();
    while (port.available() > 0) {
        int ch = port.read();

        if (synced == 0 && ch != '$') return;   // initial synchronization - also used to resync/realign if needed
        synced = 1;
        print ((char)ch);

        if ((serialCount == 1 && ch != 2)
            || (serialCount == 12 && ch != 'r')
            || (serialCount == 13 && ch != 'n'))  {
            serialCount = 0;
            synced = 0;
            return;
        }

        if (serialCount > 0 || ch == '$') {
            teapotPacket[serialCount  ] = (char)ch;
            if (serialCount == 14) {
                serialCount = 0; // restart packet byte position
                
                // get quaternion from data packet
                q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
                q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
                q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
                q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
                for (int i = 0; i < 4; i  ) if (q[i] >= 2) q[i] = -4   q[i];
                
                // set our toxilibs quaternion to new data
                quat.set(q[0], q[1], q[2], q[3]);

                
                // below calculations unnecessary for orientation only using toxilibs
                
                // calculate gravity vector
                gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
                gravity[1] = 2 * (q[0]*q[1]   q[2]*q[3]);
                gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2]   q[3]*q[3];
    
                // calculate Euler angles
                euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0]   2*q[1]*q[1] - 1);
                euler[1] = -asin(2*q[1]*q[3]   2*q[0]*q[2]);
                euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0]   2*q[3]*q[3] - 1);
    
                // calculate yaw/pitch/roll angles
                ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0]   2*q[1]*q[1] - 1);
                ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1]   gravity[2]*gravity[2]));
                ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0]   gravity[2]*gravity[2]));
    
                // output various components for debugging
                //println("q:t"   round(q[0]*100.0f)/100.0f   "t"   round(q[1]*100.0f)/100.0f   "t"   round(q[2]*100.0f)/100.0f   "t"   round(q[3]*100.0f)/100.0f);
                //println("euler:t"   euler[0]*180.0f/PI   "t"   euler[1]*180.0f/PI   "t"   euler[2]*180.0f/PI);
                //println("ypr:t"   ypr[0]*180.0f/PI   "t"   ypr[1]*180.0f/PI   "t"   ypr[2]*180.0f/PI);
                
            }
        }
    }
}

void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
    float angle = 0;
    float angleIncrement = TWO_PI / sides;
    beginShape(QUAD_STRIP);
    for (int i = 0; i < sides   1;   i) {
        vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
        vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
        angle  = angleIncrement;
    }
    endShape();
    
    // If it is not a cone, draw the circular top cap
    if (topRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
        
        // Center point
        vertex(0, 0, 0);
        for (int i = 0; i < sides   1; i  ) {
            vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
            angle  = angleIncrement;
        }
        endShape();
    }
  
    // If it is not a cone, draw the circular bottom cap
    if (bottomRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
    
        // Center point
        vertex(0, tall, 0);
        for (int i = 0; i < sides   1; i  ) {
            vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
            angle  = angleIncrement;
        }
        endShape();
    }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output

// 6/20/2012 by Jeff Rowberg <jeff@rowberg.net>

// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// Changelog:

// 2012-06-20 - initial release

/* ============================================

I2Cdev device library code is placed under the MIT license

Permission is hereby granted, free of charge, to any person obtaining a copy

of this software and associated documentation files (the "Software"), to deal

in the Software without restriction, including without limitation the rights

to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

copies of the Software, and to permit persons to whom the Software is

furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in

all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

THE SOFTWARE.

===============================================

import processing.serial.*;

import processing.opengl.*;

import toxi.geom.*;

import toxi.processing.*;

// NOTE: requires ToxicLibs to be installed in order to run properly.

// 1. Download from http://toxiclibs.org/downloads

// 2. Extract into [userdir]/Processing/libraries

// (location may be different on Mac/Linux)

// 3. Run and bask in awesomeness

ToxiclibsSupport gfx;

Serial port; // The serial port

char[] teapotPacket = new char[14]; // InvenSense Teapot packet

int serialCount = 0; // current packet byte position

int synced = 0;

int interval = 0;

float[] q = new float[4];

Quaternion quat = new Quaternion(1, 0, 0, 0);

float[] gravity = new float[3];

float[] euler = new float[3];

float[] ypr = new float[3];

PShape s;

String message = " Arduino Uno and GY-521";

PFont f;

// The radius of a circle

float r = 200;

void setup() {

// 300px square viewport using OpenGL rendering

size(800, 600, P3D);

gfx = new ToxiclibsSupport(this);

//**********

s = loadShape("f15.obj");

s.scale(14, 14, 14);

s.rotateX(0);

s.rotateY(0);

s.rotateZ(3.13);

f = createFont("Georgia",40,true);

textFont(f);

// The text must be centered!

textAlign(CENTER);

// setup lights and antialiasing

lights();

smooth(2);

// display serial port list for debugging/clarity

println(Serial.list());

// get the first available port (use EITHER this OR the specific port code below)

//String portName = Serial.list()[0];

// get a specific serial port (use EITHER this OR the first-available code above)

String portName = "COM3";

// open the serial port

port = new Serial(this, portName, 115200);

// send single character to trigger DMP init/start

// (expected by MPU6050_DMP6 example Arduino sketch)

port.write('r');

}

void draw() {

if (millis() - interval > 1000) {

// resend single character to trigger DMP init/start

// in case the MPU is halted/reset while applet is running

port.write('r');

interval = millis();

}

// black background

background(5,120,190);

// translate everything to the middle of the viewport

pushMatrix();

translate(width / 2, height / 2);

// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)

// ...and other weirdness I haven't figured out yet

//rotateY(-ypr[0]);

//rotateZ(-ypr[1]);

//rotateX(-ypr[2]);

// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)

// (axis order [1, 3, 2] and inversion [-1, 1, 1] is a consequence of

// different coordinate system orientation assumptions between Processing

// and InvenSense DMP)

float[] axis = quat.toAxisAngle();

rotate(axis[0], -axis[1], axis[3], axis[2]);

noStroke();

directionalLight(185, 200, 255, 0, 1, -1);

ambientLight(120, 125, 185, 35, 24, 90);

shape(s, 0, 0);

popMatrix();

// Start in the center and draw the circle

translate(width / 2, height / 2);

noFill();

stroke(0);

// We must keep track of our position along the curve

float arclength = 0;

// For every box

for (int i = 0; i < message.length(); i )

{

// Instead of a constant width, we check the width of each character.

char currentChar = message.charAt(i);

float w = textWidth(currentChar);

// Each box is centered so we move half the width

arclength = w/1.5;

// Angle in radians is the arclength divided by the radius

// Starting on the left side of the circle by adding PI

float theta = PI arclength / r;

pushMatrix();

// Polar to cartesian coordinate conversion

translate(r*cos(theta), r*sin(theta));

// Rotate the box

rotate(theta PI/2); // rotation is offset by 90 degrees

// Display the character

fill(0);

text(currentChar,0,0);

popMatrix();

// Move halfway again

arclength = w/1.5;

}

void serialEvent(Serial port) {

interval = millis();

while (port.available() > 0) {

int ch = port.read();

if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed

synced = 1;

print ((char)ch);

if ((serialCount == 1 && ch != 2)

|| (serialCount == 12 && ch != 'r')

|| (serialCount == 13 && ch != 'n')) {

serialCount = 0;

synced = 0;

return;

}

if (serialCount > 0 || ch == '$') {

teapotPacket[serialCount ] = (char)ch;

if (serialCount == 14) {

serialCount = 0; // restart packet byte position

// get quaternion from data packet

q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;

q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;

q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;

q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;

for (int i = 0; i < 4; i ) if (q[i] >= 2) q[i] = -4 q[i];

// set our toxilibs quaternion to new data

quat.set(q[0], q[1], q[2], q[3]);

// below calculations unnecessary for orientation only using toxilibs

// calculate gravity vector

gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);

gravity[1] = 2 * (q[0]*q[1] q[2]*q[3]);

gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] q[3]*q[3];

// calculate Euler angles

euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] 2*q[1]*q[1] - 1);

euler[1] = -asin(2*q[1]*q[3] 2*q[0]*q[2]);

euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] 2*q[3]*q[3] - 1);

// calculate yaw/pitch/roll angles

ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] 2*q[1]*q[1] - 1);

ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] gravity[2]*gravity[2]));

ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] gravity[2]*gravity[2]));

// output various components for debugging

//println("q:t" round(q[0]*100.0f)/100.0f "t" round(q[1]*100.0f)/100.0f "t" round(q[2]*100.0f)/100.0f "t" round(q[3]*100.0f)/100.0f);

//println("euler:t" euler[0]*180.0f/PI "t" euler[1]*180.0f/PI "t" euler[2]*180.0f/PI);

//println("ypr:t" ypr[0]*180.0f/PI "t" ypr[1]*180.0f/PI "t" ypr[2]*180.0f/PI);

}

void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {

float angle = 0;

float angleIncrement = TWO_PI / sides;

beginShape(QUAD_STRIP);

for (int i = 0; i < sides 1; i) {

vertex(topRadius*cos(angle), 0, topRadius*sin(angle));

vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));

angle = angleIncrement;

}

endShape();

// If it is not a cone, draw the circular top cap

if (topRadius != 0) {

angle = 0;

beginShape(TRIANGLE_FAN);

// Center point

vertex(0, 0, 0);

for (int i = 0; i < sides 1; i ) {

vertex(topRadius * cos(angle), 0, topRadius * sin(angle));

angle = angleIncrement;

}

endShape();

}

// If it is not a cone, draw the circular bottom cap

if (bottomRadius != 0) {

angle = 0;

beginShape(TRIANGLE_FAN);

// Center point

vertex(0, tall, 0);

for (int i = 0; i < sides 1; i ) {

vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));

angle = angleIncrement;

}

endShape();

}

参考资料：

Arduino UNO – Mabioca （主要参考资料）

Pi_Self_Driving_Car/测试mpu6050.md at master · makelove/Pi_Self_Driving_Car (github.com)

Arduino Playground – MPU-6050

Release 0021-complete · postspectacular/toxiclibs (github.com)

MPU-6050 6-axis accelerometer/gyroscope | I2C Device Library (i2cdevlib.com)

Download as PDF

创客：在Arduino UNO 上使用MUP6050加速度计

Related posts:

发表回复取消回复

创客：在Arduino UNO 上使用MUP6050加速度计

Related posts:

发表回复 取消回复

发表回复取消回复