陀螺儀GY-521 (MPU6050) 

Y-521 MPU6050 為全球首例整合性6軸運動處理組件，相較於多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝空間。MPU-6000整合了3軸陀螺儀、3軸加速器，並含可藉由第二個I2C端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數位運動處理(DMP: Digital Motion Processor)硬件加速引擎，由主要I2C端口以單一數據流的形式，向應用端輸出完整的9軸融合演算技術。

InvenSense 的運動處理資料庫，可處理運動感測的復雜數據，降低了運動處理運算對操作系統的負荷，並為應用開發提供架構化的API。MPU-6000的角速度全格感測範圍為±250、±500、±1000與±2000°/sec (dps)，可准確追緃快速與慢速動作，並且，用戶可程式控制的加速器全格感測範圍為±2g、±4g±8g與±16g。產品傳輸可透過最高至400kHz的I2C或最高達20MHz的SPI。

MPU-6000可在不同電壓下工作，VDD供電電壓介為2.5V±5%、3.0V±5%或3.3V±5%，邏輯接口VVDIO供電為1.8V± 5%。MPU-6000的包裝尺寸4x4x0.9mm(QFN)，在業界是革命性的尺寸。其他的特征包含內建的溫度感測器、包含在運作環境中僅有±1%變動的振蕩器。

◎使用晶片：MPU-6050

◎供電電源：3-5v（內部低壓差穩壓）

◎通信方式：標准IIC通信協議

◎晶片內置 16bit AD轉換器,16位數據輸出

◎陀螺儀範圍：±250 500 1000 2000 °/s

◎加速度範圍：±2±4±8±16g

◎採用沉金PCB，機器焊接工藝保證質量

◎引腳間距2.54mm

include <Wire.h>

void setup()

{

  Serial.begin (115200);  

  Wire.begin (21, 22);   // sda= GPIO_21 /scl= GPIO_22

}

void Scanner ()

{

  Serial.println ();

  Serial.println ("I2C scanner. Scanning ...");

  byte count = 0;

  Wire.begin();

  for (byte i = 8; i < 120; i++)

  {

    Wire.beginTransmission (i); // Begin I2C transmission Address (i)

    if (Wire.endTransmission () == 0) //0=success(ACK response) 

    {

      Serial.print ("Found address: ");

      Serial.print (i, DEC);

      Serial.print (" (0x");

      Serial.print (i, HEX);     // PCF8574 7 bit address

      Serial.println (")");

      count++;

    }

  }

  Serial.print ("Found ");      

  Serial.print (count, DEC);        // numbers of devices

  Serial.println (" device(s).");

}

void loop()

{

  Scanner ();

  delay (5000);

}

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class

// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>

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

//

// Changelog:

//      2013-05-08 - added multiple output formats

//                 - added seamless Fastwire support

//      2011-10-07 - initial release

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

I2Cdev device library code is placed under the MIT license

Copyright (c) 2011 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.

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

*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files

// for both classes must be in the include path of your project

#include "I2Cdev.h"

#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

    #include "Wire.h"

#endif

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for InvenSense evaluation board)

// AD0 high = 0x69

MPU6050 accelgyro;

//MPU6050 accelgyro(0x69); // <-- use for AD0 high

//MPU6050 accelgyro(0x68, &Wire1); // <-- use for AD0 low, but 2nd Wire (TWI/I2C) object

int16_t ax, ay, az;

int16_t gx, gy, gz;

// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated

// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,

// not so easy to parse, and slow(er) over UART.

#define OUTPUT_READABLE_ACCELGYRO

// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit

// binary, one right after the other. This is very fast (as fast as possible

// without compression or data loss), and easy to parse, but impossible to read

// for a human.

//#define OUTPUT_BINARY_ACCELGYRO

#define LED_PIN 13

bool blinkState = false;

void setup() {

    // join I2C bus (I2Cdev library doesn't do this automatically)

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

        Wire.begin();

    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

        Fastwire::setup(400, true);

    #endif

    // initialize serial communication

    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but

    // it's really up to you depending on your project)

    Serial.begin(115200);

    // initialize device

    Serial.println("Initializing I2C devices...");

    accelgyro.initialize();

    // verify connection

    Serial.println("Testing device connections...");

    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // use the code below to change accel/gyro offset values

    /*

    Serial.println("Updating internal sensor offsets...");

    // -76 -2359 1688 0 0 0

    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76

    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359

    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688

    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0

    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0

    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0

    Serial.print("\n");

    accelgyro.setXGyroOffset(220);

    accelgyro.setYGyroOffset(76);

    accelgyro.setZGyroOffset(-85);

    Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76

    Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359

    Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688

    Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0

    Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0

    Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0

    Serial.print("\n");

    */

    // configure Arduino LED pin for output

    pinMode(LED_PIN, OUTPUT);

}

void loop() {

    // read raw accel/gyro measurements from device

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // these methods (and a few others) are also available

    //accelgyro.getAcceleration(&ax, &ay, &az);

    //accelgyro.getRotation(&gx, &gy, &gz);

    #ifdef OUTPUT_READABLE_ACCELGYRO

        // display tab-separated accel/gyro x/y/z values

        Serial.print("a/g:\t");

        Serial.print(ax); Serial.print("\t");

        Serial.print(ay); Serial.print("\t");

        Serial.print(az); Serial.print("\t");

        Serial.print(gx); Serial.print("\t");

        Serial.print(gy); Serial.print("\t");

        Serial.println(gz);

    #endif

    #ifdef OUTPUT_BINARY_ACCELGYRO

        Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));

        Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));

        Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));

        Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));

        Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));

        Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));

    #endif

    // blink LED to indicate activity

    blinkState = !blinkState;

    digitalWrite(LED_PIN, blinkState);

    delay(100);

}

// 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];

void setup() {

    // 300px square viewport using OpenGL rendering

    size(300, 300, OPENGL);

    gfx = new ToxiclibsSupport(this);

    // setup lights and antialiasing

    lights();

    smooth();

    // 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 = "COM6";

    // 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(0);

    // 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]);

    // draw main body in red

    fill(255, 0, 0, 200);

    box(10, 10, 200);

    // draw front-facing tip in blue

    fill(0, 0, 255, 200);

    pushMatrix();

    translate(0, 0, -120);

    rotateX(PI/2);

    drawCylinder(0, 20, 20, 8);

    popMatrix();

    // draw wings and tail fin in green

    fill(0, 255, 0, 200);

    beginShape(TRIANGLES);

    vertex(-100,  2, 30); vertex(0,  2, -80); vertex(100,  2, 30);  // wing top layer

    vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30);  // wing bottom layer

    vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70);  // tail left layer

    vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70);  // tail right layer

    endShape();

    beginShape(QUADS);

    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);

    vertex( 100, 2, 30); vertex( 100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);

    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);

    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);

    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2,   0, 70); vertex(-2,   0, 70);

    vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   0, 70); vertex(-2,   0, 70);

    endShape();

    popMatrix();

}

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();

    }

}