在本篇文章中，我们将主要介绍如何将MPU6050加速度计和陀螺仪传感器与Arduino开发板一起使用。首先，我们将介绍MPU6050是如何工作的以及如何从中读取数据，然后我们制作两个实际的例子。

概览

在第一个例子中，使用Processing开发环境，我们将对传感器方向进行三维可视化，在第二个例子中，我们将制作一个简单的自平衡平台或DIY万向节。基于MPU6050方向及其融合加速度计和陀螺仪数据，我们控制三个保持平台水平的伺服系统。

MPU6050的工作过程

MPU6050 IMU在单芯片上集成了一个3轴加速度计和一个3轴陀螺仪。

陀螺仪沿X、Y和Z轴测量角位置随时间的旋转速度或变化率。它使用MEMS技术和科里奥利效应进行测量。陀螺仪的输出以每秒度数为单位，因此为了获得角度位置，我们只需要对角速度进行积分。

MPU6050 IMU 3轴加速度计和3轴陀螺仪

另一方面，MPU6050加速度计测量加速度的方式与ADXL345加速度传感器相同。简而言之，它可以测量沿3个轴的重力加速度，并使用一些三角学数学，我们可以计算传感器定位的角度。因此，如果我们融合或组合加速度计和陀螺仪数据，我们可以获得有关传感器方向的非常准确的信息。

MPU6050 IMU也称为六轴运动跟踪设备或6 DoF（六自由度）设备，因为它有6个输出，即3个加速度计输出和3个陀螺仪输出。

Arduino和MPU6050的连接方法

我们来看看如何使用Arduino连接和读取MPU6050传感器的数据。我们使用I2C协议与Arduino进行通信，因此只需要两条线进行连接，另外还有两条线用于供电。

Arduino和MPU6050连接电路图

MPU6050的Arduino代码

以下是用于从MPU6050传感器读取数据的Arduino代码。在代码下方，您可以找到它的详细说明。

1. /*
   
2.    Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
   
3. */
   
4. #include <Wire.h>
   
5. const int MPU = 0x68; // MPU6050 I2C address
   
6. float AccX, AccY, AccZ;
   
7. float GyroX, GyroY, GyroZ;
   
8. float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
   
9. float roll, pitch, yaw;
   
10. float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
    
11. float elapsedTime, currentTime, previousTime;
    
12. int c = 0;
    
13. void setup() {
    
14.   Serial.begin(19200);
    
15.   Wire.begin();                      // Initialize comunication
    
16.   Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
    
17.   Wire.write(0x6B);                  // Talk to the register 6B
    
18.   Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
    
19.   Wire.endTransmission(true);        //end the transmission
    
20.   /*
    
21.   // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
    
22.   Wire.beginTransmission(MPU);
    
23.   Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
    
24.   Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
    
25.   Wire.endTransmission(true);
    
26.   // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
    
27.   Wire.beginTransmission(MPU);
    
28.   Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
    
29.   Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
    
30.   Wire.endTransmission(true);
    
31.   delay(20);
    
32.   */
    
33.   // Call this function if you need to get the IMU error values for your module
    
34.   calculate_IMU_error();
    
35.   delay(20);
    
36. }
    
37. void loop() {
    
38.   // === Read acceleromter data === //
    
39.   Wire.beginTransmission(MPU);
    
40.   Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
    
41.   Wire.endTransmission(false);
    
42.   Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
    
43.   //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
    
44.   AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
    
45.   AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
    
46.   AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
    
47.   // Calculating Roll and Pitch from the accelerometer data
    
48.   accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
    
49.   accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
    
50.   // === Read gyroscope data === //
    
51.   previousTime = currentTime;        // Previous time is stored before the actual time read
    
52.   currentTime = millis();            // Current time actual time read
    
53.   elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
    
54.   Wire.beginTransmission(MPU);
    
55.   Wire.write(0x43); // Gyro data first register address 0x43
    
56.   Wire.endTransmission(false);
    
57.   Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
    
58.   GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
    
59.   GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    
60.   GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
    
61.   // Correct the outputs with the calculated error values
    
62.   GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
    
63.   GyroY = GyroY - 2; // GyroErrorY ~(2)
    
64.   GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
    
65.   // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
    
66.   gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
    
67.   gyroAngleY = gyroAngleY + GyroY * elapsedTime;
    
68.   yaw =  yaw + GyroZ * elapsedTime;
    
69.   // Complementary filter - combine acceleromter and gyro angle values
    
70.   roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
    
71.   pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
    
72.   
    
73.   // Print the values on the serial monitor
    
74.   Serial.print(roll);
    
75.   Serial.print("/");
    
76.   Serial.print(pitch);
    
77.   Serial.print("/");
    
78.   Serial.println(yaw);
    
79. }
    
80. void calculate_IMU_error() {
    
81.   // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
    
82.   // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
    
83.   // Read accelerometer values 200 times
    
84.   while (c < 200) {
    
85.     Wire.beginTransmission(MPU);
    
86.     Wire.write(0x3B);
    
87.     Wire.endTransmission(false);
    
88.     Wire.requestFrom(MPU, 6, true);
    
89.     AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
90.     AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
91.     AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
92.     // Sum all readings
    
93.     AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    
94.     AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    
95.     c++;
    
96.   }
    
97.   //Divide the sum by 200 to get the error value
    
98.   AccErrorX = AccErrorX / 200;
    
99.   AccErrorY = AccErrorY / 200;
    
100.   c = 0;
     
101.   // Read gyro values 200 times
     
102.   while (c < 200) {
     
103.     Wire.beginTransmission(MPU);
     
104.     Wire.write(0x43);
     
105.     Wire.endTransmission(false);
     
106.     Wire.requestFrom(MPU, 6, true);
     
107.     GyroX = Wire.read() << 8 | Wire.read();
     
108.     GyroY = Wire.read() << 8 | Wire.read();
     
109.     GyroZ = Wire.read() << 8 | Wire.read();
     
110.     // Sum all readings
     
111.     GyroErrorX = GyroErrorX + (GyroX / 131.0);
     
112.     GyroErrorY = GyroErrorY + (GyroY / 131.0);
     
113.     GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
     
114.     c++;
     
115.   }
     
116.   //Divide the sum by 200 to get the error value
     
117.   GyroErrorX = GyroErrorX / 200;
     
118.   GyroErrorY = GyroErrorY / 200;
     
119.   GyroErrorZ = GyroErrorZ / 200;
     
120.   // Print the error values on the Serial Monitor
     
121.   Serial.print("AccErrorX: ");
     
122.   Serial.println(AccErrorX);
     
123.   Serial.print("AccErrorY: ");
     
124.   Serial.println(AccErrorY);
     
125.   Serial.print("GyroErrorX: ");
     
126.   Serial.println(GyroErrorX);
     
127.   Serial.print("GyroErrorY: ");
     
128.   Serial.println(GyroErrorY);
     
129.   Serial.print("GyroErrorZ: ");
     
130.   Serial.println(GyroErrorZ);
     
131. }

复制代码

代码描述：首先我们需要包含用于I2C通信的Wire.h库，并定义一些存储数据所需的变量。

在setup()函数部分，我们需要初始化wire库并通过电源管理寄存器复位传感器。 为此，我们需要查看传感器的数据手册，从中我们可以看到寄存器地址。

MPU6050电源管理寄存器0x6B

此外，如果需要，我们可以使用配置寄存器为加速度计和陀螺仪选择满量程范围。 对于这个例子，我们将使用加速度计的默认±2g范围和陀螺仪的250度/秒范围。

1. // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
   
2.   Wire.beginTransmission(MPU);
   
3.   Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
   
4.   Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
   
5.   Wire.endTransmission(true);
   
6.   // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
   
7.   Wire.beginTransmission(MPU);
   
8.   Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
   
9.   Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
   
10.   Wire.endTransmission(true);
    
11.   */

复制代码

在loop()函数部分，我们首先读取加速度计的数据。 每个轴的数据存储在两个字节或寄存器中，我们可以从传感器的数据手册中看到这些寄存器的地址。

MPU6050 imu加速度计数据寄存器

为了全部读取它们，我们从第一个寄存器开始，然后使用requiestFrom()函数，我们请求读取X、Y和Z轴的所有6个寄存器。 然后我们从每个寄存器读取数据，并且由于输出是二进制补码，我们将它们相应地组合以获得正确的值。

1. // === Read acceleromter data === //
   
2.   Wire.beginTransmission(MPU);
   
3.   Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
   
4.   Wire.endTransmission(false);
   
5.   Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
   
6.   //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
   
7.   AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
   
8.   AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
   
9.   AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value

复制代码

为了获得-1g到+ 1g的输出值，适合计算角度，我们将输出除以先前选择的灵敏度。

mpu6050加速度计灵敏度满量程范围

最后，使用这两个公式，我们从加速度计数据计算滚转角和俯仰角。

1.   accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
   
2.   accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)

复制代码

接下来，使用相同的方法我们得到陀螺仪数据。

我们读取了六个陀螺仪寄存器，适当地组合它们的数据并将其除以先前选择的灵敏度，以便以每秒的度数获得输出。

1. // === Read gyroscope data === //
   
2.   previousTime = currentTime;        // Previous time is stored before the actual time read
   
3.   currentTime = millis();            // Current time actual time read
   
4.   elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
   
5.   Wire.beginTransmission(MPU);
   
6.   Wire.write(0x43); // Gyro data first register address 0x43
   
7.   Wire.endTransmission(false);
   
8.   Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
   
9.   GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
   
10.   GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    
11.   GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

复制代码

在这里你可以注意到我用一些小的计算误差值来校正输出值，我将在接下来解释它们是如何得到它们的。 因此，当输出以度/秒为单位时，现在我们需要将它们与时间相乘以得到度数。 使用millis()函数在每次读取迭代之前捕获时间值。

1. // Correct the outputs with the calculated error values
   
2.   GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
   
3.   GyroY = GyroY - 2; // GyroErrorY ~(2)
   
4.   GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
   
5.   // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
   
6.   gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
   
7.   gyroAngleY = gyroAngleY + GyroY * elapsedTime;
   
8.   yaw =  yaw + GyroZ * elapsedTime;

复制代码

最后，我们使用互补滤波器融合加速度计和陀螺仪数据。在这里，我们采用96％的陀螺仪数据，因为它非常准确，不会受到外力的影响。陀螺仪的缺点是存在漂移，或者随着时间的推移在输出中引入误差。因此，从长远来看，我们使用来自加速度计的数据，本例为4％，足以消除陀螺仪的漂移误差。

1. // Complementary filter - combine acceleromter and gyro angle values
   
2.   roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
   
3.   pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

复制代码

但是，由于我们无法从加速度计数据计算偏航，我们无法在其上实现互补滤波器。

在我们看一下结果之前，让我快速解释一下如何获得纠错值。为了计算这些错误，我们可以在传感器处于平坦静止位置时调用calculate_IMU_error()自定义函数。在这里，我们为所有输出做了200个读数，我们将它们相加并将它们除以200。由于我们将传感器保持在平坦静止位置，因此预期输出值应为0。因此，通过此计算，我们可以得到传感器的平均误差。

1. void calculate_IMU_error() {
   
2.   // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
   
3.   // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
   
4.   // Read accelerometer values 200 times
   
5.   while (c < 200) {
   
6.     Wire.beginTransmission(MPU);
   
7.     Wire.write(0x3B);
   
8.     Wire.endTransmission(false);
   
9.     Wire.requestFrom(MPU, 6, true);
   
10.     AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
11.     AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
12.     AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    
13.     // Sum all readings
    
14.     AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    
15.     AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    
16.     c++;
    
17.   }
    
18.   //Divide the sum by 200 to get the error value
    
19.   AccErrorX = AccErrorX / 200;
    
20.   AccErrorY = AccErrorY / 200;
    
21.   c = 0;
    
22.   // Read gyro values 200 times
    
23.   while (c < 200) {
    
24.     Wire.beginTransmission(MPU);
    
25.     Wire.write(0x43);
    
26.     Wire.endTransmission(false);
    
27.     Wire.requestFrom(MPU, 6, true);
    
28.     GyroX = Wire.read() << 8 | Wire.read();
    
29.     GyroY = Wire.read() << 8 | Wire.read();
    
30.     GyroZ = Wire.read() << 8 | Wire.read();
    
31.     // Sum all readings
    
32.     GyroErrorX = GyroErrorX + (GyroX / 131.0);
    
33.     GyroErrorY = GyroErrorY + (GyroY / 131.0);
    
34.     GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    
35.     c++;
    
36.   }
    
37.   //Divide the sum by 200 to get the error value
    
38.   GyroErrorX = GyroErrorX / 200;
    
39.   GyroErrorY = GyroErrorY / 200;
    
40.   GyroErrorZ = GyroErrorZ / 200;
    
41.   // Print the error values on the Serial Monitor
    
42.   Serial.print("AccErrorX: ");
    
43.   Serial.println(AccErrorX);
    
44.   Serial.print("AccErrorY: ");
    
45.   Serial.println(AccErrorY);
    
46.   Serial.print("GyroErrorX: ");
    
47.   Serial.println(GyroErrorX);
    
48.   Serial.print("GyroErrorY: ");
    
49.   Serial.println(GyroErrorY);
    
50.   Serial.print("GyroErrorZ: ");
    
51.   Serial.println(GyroErrorZ);
    
52. }

复制代码

我们只需在串行监视器上打印这些值，一旦我们知道它们，我们就可以在前面所示的代码中实现它们，用于滚动和俯仰计算，以及3个陀螺仪输出。

MPU6050俯仰滚转和偏航输出

最后，使用Serial.print函数，我们可以在串行监视器上打印Roll、Pitch和Yaw值，看看传感器是否正常工作。

MPU6050方向跟踪 - 三维可视化

接下来，为了制作3D可视化示例，我们只需要接收Arduino通过Processing开发环境中的串行端口发送的数据。 以下是完整的Processing代码：

1. /*
   
2.     Arduino and MPU6050 IMU - 3D Visualization Example
   
3. */
   
4. import processing.serial.*;
   
5. import java.awt.event.KeyEvent;
   
6. import java.io.IOException;
   
7. Serial myPort;
   
8. String data="";
   
9. float roll, pitch,yaw;
   
10. void setup() {
    
11.   size (2560, 1440, P3D);
    
12.   myPort = new Serial(this, "COM7", 19200); // starts the serial communication
    
13.   myPort.bufferUntil('\n');
    
14. }
    
15. void draw() {
    
16.   translate(width/2, height/2, 0);
    
17.   background(233);
    
18.   textSize(22);
    
19.   text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
    
20.   // Rotate the object
    
21.   rotateX(radians(-pitch));
    
22.   rotateZ(radians(roll));
    
23.   rotateY(radians(yaw));
    
24.   
    
25.   // 3D 0bject
    
26.   textSize(30);  
    
27.   fill(0, 76, 153);
    
28.   box (386, 40, 200); // Draw box
    
29.   textSize(25);
    
30.   fill(255, 255, 255);
    
31.   text("www.HowToMechatronics.com", -183, 10, 101);
    
32.   //delay(10);
    
33.   //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
    
34. }
    
35. // Read data from the Serial Port
    
36. void serialEvent (Serial myPort) {
    
37.   // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
    
38.   data = myPort.readStringUntil('\n');
    
39.   // if you got any bytes other than the linefeed:
    
40.   if (data != null) {
    
41.     data = trim(data);
    
42.     // split the string at "/"
    
43.     String items[] = split(data, '/');
    
44.     if (items.length > 1) {
    
45.       //--- Roll,Pitch in degrees
    
46.       roll = float(items[0]);
    
47.       pitch = float(items[1]);
    
48.       yaw = float(items[2]);
    
49.     }
    
50.   }
    
51. }

复制代码

代码中，我们读取Arduino的输入数据并将其放入相应的变量Roll、Pitch和Yaw中。在draw函数中，我们使用这些值来旋转3D对象，本例中是一个带有特定颜色和文本的简单框。

如果我们运行草图，可以看到MPU6050传感器跟踪方向有多好。 3D物体跟踪传感器的方向非常准确，并且响应速度也非常快。

MPU6050方向跟踪 - 三维可视化示例

正如我所提到的，唯一的缺点是偏航会随着时间的推移而漂移，因为我们不能使用互补滤波器。为了改善这一点，我们需要使用额外的传感器。通常是一个磁力计，可用作陀螺仪偏航漂移的长期校正。然而，MPU6050实际上具有一种称为数字运动处理器（Digital Motion Processor）的功能，用于数据的板载计算，并且能够消除偏航漂移。

绝对定向传感器 -  MPU6050 BNO055

这是使用数字运动处理器的相同3D示例。我们可以看到现在的方向跟踪有多精确，没有偏航漂移。板载处理器还可以计算和输出四元数Quaternions，这些四元数用于表示三维对象的方向和旋转。在这个例子中，我们实际上使用四元数来表示方向，这也不会受到使用欧拉角（Euler angles)时发生的万向节Gimbal锁定的影响。

使用MPU6050 DMP功能进行3D对象定向

然而，从传感器获取这些数据比我们之前解释的要复杂一些。首先，我们需要连接和附加导线到Arduino数字引脚。这是一个中断引脚，用于从MPU6050读取此数据类型。

代码也有点复杂，所以我们将使用Jeff Rowberg的i2cdevlib库。这个库可以从GitHub下载。安装库后，我们可以从库中打开MPU6050_DMP6示例。每个行的注释都很好地解释了这个例子。

示例中，我们可以选择想要的输出类型，四元数、欧拉角、偏航、俯仰和滚转、加速度值或四元数，用于三维可视化。该库还包括3D可视化示例的Processing草图。我刚修改它以获得前一个例子中的盒子形状。以下是与MPU6050_DPM6示例一起使用的3D可视化处理代码，用于选择的“OUTPUT_TEAPOT”输出：

1. /*
   
2.     Arduino and MPU6050 IMU - 3D Visualization Example
   
3. */
   
4. import processing.serial.*;
   
5. import java.awt.event.KeyEvent;
   
6. import java.io.IOException;
   
7. Serial myPort;
   
8. String data="";
   
9. float roll, pitch,yaw;
   
10. void setup() {
    
11.   size (2560, 1440, P3D);
    
12.   myPort = new Serial(this, "COM7", 19200); // starts the serial communication
    
13.   myPort.bufferUntil('\n');
    
14. }
    
15. void draw() {
    
16.   translate(width/2, height/2, 0);
    
17.   background(233);
    
18.   textSize(22);
    
19.   text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
    
20.   // Rotate the object
    
21.   rotateX(radians(-pitch));
    
22.   rotateZ(radians(roll));
    
23.   rotateY(radians(yaw));
    
24.   
    
25.   // 3D 0bject
    
26.   textSize(30);  
    
27.   fill(0, 76, 153);
    
28.   box (386, 40, 200); // Draw box
    
29.   textSize(25);
    
30.   fill(255, 255, 255);
    
31.   text("www.HowToMechatronics.com", -183, 10, 101);
    
32.   //delay(10);
    
33.   //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
    
34. }
    
35. // Read data from the Serial Port
    
36. void serialEvent (Serial myPort) {
    
37.   // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
    
38.   data = myPort.readStringUntil('\n');
    
39.   // if you got any bytes other than the linefeed:
    
40.   if (data != null) {
    
41.     data = trim(data);
    
42.     // split the string at "/"
    
43.     String items[] = split(data, '/');
    
44.     if (items.length > 1) {
    
45.       //--- Roll,Pitch in degrees
    
46.       roll = float(items[0]);
    
47.       pitch = float(items[1]);
    
48.       yaw = float(items[2]);
    
49.     }
    
50.   }
    
51. }

复制代码

所以这里我们使用serialEvent()函数接收来自Arduino的四元数，并且在主draw函数循环中，我们使用这些值来旋转3D对象。 如果我们运行草图，就可以看到四元数对于三维旋转对象有多好。

第二个例子，自制Arduino万向节或自稳定平台，我们将放在另一篇文章中进行介绍。

代码实测可用，佩服楼主的坚持，每一篇文章都是长篇大论，能从中完全了解每一步的意思