index
第1部分：简介
这是一个很好的问题，答案更好！自平衡系统在很多地方都可以看到，它们对于多种机器的平稳运行至关重要。一些明显的例子包括赛格威，双足机器人和太空火箭（由于平衡系统故障，一些火箭丢失了 ）。
但是，许多人没有意识到的是，这些系统使用的控制器与伺服电机，空调甚至恒温器中的控制器相同。当然，火箭使用的控制器比空调要复杂得多，但是其基本原理仍然是相同的：如何调整系统以尽可能接近所需的结果。这就是为什么建造一个自我平衡的机器人如此具有教育意义的原因；您可以一遍又一遍地对其他项目使用相同的控制方法。而且请不要忘记，自平衡机器人是一个非常有趣的玩具！这是我自我平衡的视频：
1.1 简要概述
构建自平衡机器人需要采取许多步骤。最简单的部分是硬件，因此始终是一个不错的起点。该机器人需要两个电机，一个电机控制器，一个陀螺仪，一个微控制器和某种类型的框架。令人惊讶的是，由于倒立摆的惯性 ，机器人实际上发现，如果框架很高而顶部重很多，而不是重心较低的小框架，则更容易稳定！
一旦所有的物理组件都组装好了，我们就可以进行棘手的工作了。该软件。由于系统天生不稳定，因此微控制器需要不断刷新自身，并尽可能保持最快速度和效率，以保持其平衡。因此，循环时间应尽可能短（但仍保持常规）。对于我的机器人，我使用了10ms的标准循环时间。因此，控制器每秒重新计算其响应100次！
我的自平衡机器人的马达
1.2 所需组件
在深入研究如何制造自平衡机器人之前，我们需要将硬件整合在一起。这是所需组件的列表，并带有一些链接作为参考：
必要：
1个Arduino兼容控制器
1 x电源/电池
1 x 马达控制器
1个陀螺仪/加速度计模块（例如MPU-6050）
2 x 高扭矩变速箱电机
颜色编码的电线，最好是单芯
某种类型的框架！
可选的：
一些控制机器人的按钮
开/关电源开关
多种彩色LED
上面的链接仅供参考！我建议您在购买任何东西之前先到多家公司看看。
我决定将Intel Galileo Gen2开发板用于我的微控制器，并且它与Arduino环境兼容。实际上，您可以使用任何板（例如Uno，Mega，Red-board）制作自平衡机器人。在我所有的机器人中，我都使用了MPU-6050传感器，该传感器同时包含陀螺仪和加速度计。将两个传感器一起使用的好处是可以将它们的数据融合在一起，从而使我们的读数更加稳定和准确。
为了创建框架，我使用SketchUp在计算机上设计了所有组件，然后将它们发送3D打印。我必须承认，这是相当昂贵的，所以一种替代方法是用废旧零件构建自己的框架！我用棒棒糖棒和胶水制作了我的第一个自平衡机器人原型，我很惊讶它的效果如何！常见的框架设计是使用几个塑料/木质矩形作为平台，并使用长螺纹螺栓和一些螺母将它们连接起来。
 |  |  |
第2部分：加速度计和陀螺仪传感器
如果您尚未阅读本系列自平衡机器人系列的第一部分，我鼓励您现在就阅读！下面的第一部分讨论了传感器背后的一些理论，因此，如果您想直接进入编程/构建部分，请随时跳至第二部分…
2.1 确定要使用的陀螺仪模块
因此，您决定继续进行该项目，并构造自己的自平衡机器人？那很棒！现在，我们可以看一下该项目最重要的元素之一。传感器。传统上，偏爱稳定的传感器是陀螺仪。如今的陀螺仪非常小巧，而且价格便宜，因此非常适合业余电子项目。不幸的是，这些陀螺仪（便宜的和不太便宜的版本）也都有自己的问题。它们对于短期和快速运动很有用，但是随着误差的累积，它们会随着时间的流逝而漂移。它们还记录了大量的抖动和噪声，这些抖动和噪声需要在使用数据之前由微控制器过滤掉。
为了减少陀螺仪的这种漂移影响，可以将传感器数据与来自加速度计的数据进行组合。加速度计擅长于检测较慢和较长的运动，而不是快速运动。因此，如果我们两全其美，将数据融合在一起，我们将获得机器人运动的极其精确的图像。
因此，我决定使用组合的加速度计和陀螺仪突破模块（MPU-6050），该模块比简单的陀螺仪稍微贵一些，但应该具有出色的稳定性能。注意：MPU-6050带有一个库，可以为您完成所有计算和过滤，因此绝对是加分项！
2.2 Accel-Gyro模块入门
所述MPU-6050使用I2C与微控制器进行通信，因此我开始通过如图所示的原理图的连接起来的引脚：SDA线连接到模拟脚4，SCL到模拟脚5，电源输入到3.3v引脚，接地到GND引脚。
加速 - 陀螺仪与Galileo的原理图接线。（使用Fritzing制作）
由于我使用的是英特尔第二代Galileo板，我没有使用中断引脚。通常，忽略中断是非常不好的做法，因为它可能导致保存传感器数据的缓冲区溢出，但是由于某些原因，Galileo不支持正常中断！为了使传感器在Galileo板上正常工作，我花了很多时间在获取正确的时间上，以防止传入的传感器数据溢出。对于所有其他Arduino兼容的板，应该使用中断以便用微控制器发送的新的传感器数据。（中断引脚连接到Arduino I / O引脚2）
现在是时候从accel-gyro模块获取一些数据了！为此，我仅使用MPU6050文档随附的示例代码来读取原始传感器数据。为了使该示例正常工作， 需要安装I2Cdev 和MPU6050库。这是代码：
// 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
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.
/*
如果要查看以制表符分隔的以十进制表示的 加速度X / Y / Z和陀螺仪X / Y / Z值，请取消注释“ OUTPUT_READABLE_ACCELGYRO”。 易于阅读，但不易解析，并且降低UART上速度。
*/
#define OUTPUT_READABLE_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)
/*
（之所以选择38400，是因为它在8MHz时和16MHz时都工作得很好，但实际上取决于您的项目）
*/
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.print("MPU Connection ");
Serial.println(accelgyro.testConnection() ? "successful" : "failed");
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
void loop() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &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
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
我在串行监视器上得到的结果如下所示：
Initializing I2C devices...
Testing device connections...
MPU Connection successful
a/g: -1428 14240 12120 -536 131 -149
a/g: -1416 14196 11972 -505 110 -169
a/g: -1484 14260 11948 -524 108 -147
a/g: -1508 14220 11968 -513 87 -151
a/g: -1540 14176 11920 -501 15 -164
a/g: -1408 14212 11984 -523 27 -153
a/g: -1472 14104 11888 -526 123 -137
a/g: -1400 14236 11936 -514 136 -148
a/g: -1512 14216 12008 -522 132 -142
a/g: -1412 14172 11956 -520 127 -158
a/g: -1456 14120 11936 -533 94 -166
a/g: -1496 14124 11936 -529 97 -161
a/g: -1496 14208 11996 -526 108 -177
a/g: -1420 14236 11992 -505 104 -151
a/g: -1540 14264 11984 -510 115 -171
a/g: -1468 14300 12068 -504 51 -143
a/g: -1508 14172 11868 -545 105 -137
a/g: -1416 14244 11812 -523 69 -154
a/g: -1472 14276 11956 -504 92 -146
a/g: -1492 14192 12028 -517 143 -175
2.3 标记
在此数据中，我们可以看到加速度计的读数已分为x / y / z值，而陀螺仪的读数已分为其x / y / z分量。这是迈出的重要第一步，但不幸的是，这些数据以其当前形式并不是很有用。我们仍然必须将加速度计和陀螺仪数据融合在一起，然后对其进行滤波以消除所有噪声！
MPU6050模块连接到Galileo Gen 2板上
2.4 处理数据
可以通过使用许多公式来手动计算传感器的倾斜度，但是幸运的是（至少对于MPU-6050）有一个库可以为我们做这件事！实际上，MPU-6050具有内置的“运动处理单元”（因此带有缩写），可用于处理传感器数据，从而最大程度地减少了微处理器的负担。该库还自动过滤数据，以便我们立即获得干净且可用的结果。
t a n − 1 A x A y = s i n − 1 A x A x 2 + A y 2 = s i n − 1 A x G tan ^ {-1} \frac {A_x} {A_y} = sin ^ {-1} \frac {A_x} {\sqrt {A ^ 2_x + A ^ 2_y}} = sin ^ {-1} \frac {A_x }{G} tan−1AyAx=sin−1Ax2+Ay2Ax=sin−1GAx
上面是根据加速度计数据计算倾斜角度的公式。当我选择使用lazier方法（使用库）时，我认为我无法向您解释所有这些公式。如果要了解手动计算角度的信息，请访问：http : //www.kerrywong.com/2012/03/08/a-self-balancing-robot-i/。Werry Wong在描述整个系统方面做得非常出色，所以我鼓励您检查一下！他涵盖了所有主要主题，例如计算，传感器融合和卡尔曼滤波器。
关于过滤，这是另一篇很棒的文章，探讨了使用免费过滤器代替MPU6050 motion-apps库的主要优点和缺点：http : //www.geekmomprojects.com/mpu-6050-redux-dmp -data-fusion-vs-complementary-filter /
第3部分：校准MPU6050
在本教程的这一部分中，我将介绍如何使用Motion Apps库从MPU-6050加速度计和陀螺仪模块中获得最佳性能。作者对MPU-6050进行了反向工程，因此该库的确令人惊叹，从而使大多数处理工作都是通过模块本身上的DMP完成的，而不是由微控制器完成的！我发现通过Motion Apps Library读取传感器比手动进行计算要快，并且读数往往会更加准确。
3.1 分析输出
这是我用来获取偏航，俯仰和侧倾传感器数据的代码。它基于MPU-6050 Motion Apps库随附的“ Teapot”演示程序。再次，您需要安装I2Cdev 和MPU6050库。。
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// 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_6Axis_MotionApps20.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 SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
/* =================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050's INT pin being connected to the Arduino's
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
For the Galileo Gen1/2 Boards, there is no INT pin support. Therefore
the INT pin does not need to be connected, but you should work on getting
the timing of the program right, so that there is no buffer overflow.
* ========================================================================= */
/* =========================================================================
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
when using Serial.write(buf, len). The Teapot output uses this method.
The solution requires a modification to the Arduino USBAPI.h file, which
is fortunately simple, but annoying. This will be fixed in the next IDE
release. For more info, see these links:
http://arduino.cc/forum/index.php/topic,109987.0.html
http://code.google.com/p/arduino/issues/detail?id=958
* ========================================================================= */
#define OUTPUT_READABLE_YAWPITCHROLL
// Unccomment if you are using an Arduino-Style Board
// #define ARDUINO_BOARD
// Uncomment if you are using a Galileo Gen1 / 2 Board
#define GALILEO_BOARD
#define LED_PIN 13 // (Galileo/Arduino is 13)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
VectorFloat gravity; // [x, y, z] gravity vector
Quaternion q; // [w, x, y, z] quaternion container
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
// This function is not required when using the Galileo
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
int TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial);
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(F("MPU6050 connection "));
Serial.print(mpu.testConnection() ? F("successful") : F("failed"));
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
#ifdef ARDUINO_BOARD
while (!mpuInterrupt && fifoCount < packetSize) {
}
#endif
#ifdef GALILEO_BOARD
delay(10);
#endif
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180/M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
从许多试验中，我注意到加速度计-陀螺仪模块和Motion Apps库似乎具有某种类型的自动校准功能，这需要几秒钟才能完成。这是启动传感器后直接获得的输出图：
使用Motion Apps从MPU-6050输出
所有值在开始时都会大量浮动，尤其是偏航数据。然后大约13秒后停止，这可能是由于某些自动校准过程的完成。我重复了几次测试，在某些情况下，传感器可能需要40秒才能完成校准。因此，我们应该在程序中考虑此延迟。在开始使用传感器并启动主程序之前，机器人应等待约40秒钟。
3.2 校准传感器
像任何传感器一样，MPU-6050在首次使用前也需要进行校准。我们要做的是消除零误差；这是传感器完全水平的位置，但它认为它略微倾斜。因此，我们需要调整偏移量以抵消此错误。幸运的是，我找到了一个可以为我们校准MPU-6050的程序！为了使用该程序，您要做的就是上传草图，然后将accel-gyro模块放在平坦和水平的位置。该程序将平均读取数百个读数，并显示消除零误差所需的偏移量。
// Arduino sketch that returns calibration offsets for MPU6050
// Version 1.1 (31th January 2014)
// Done by Luis Ródenas <luisrodenaslorda@gmail.com>
// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>
// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors.
// The effect of temperature has not been taken into account so I can't promise that it will work if you
// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.
// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
/// CONFIGURATION /
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone=8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone=1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// 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(0x68); // <-- use for AD0 high
int16_t ax, ay, az,gx, gy, gz;
int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
/// SETUP
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
// COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.
// initialize serial communication
Serial.begin(115200);
// initialize device
accelgyro.initialize();
// wait for ready
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()){
Serial.println(F("Send any character to start sketch.\n"));
delay(1500);
}
while (Serial.available() && Serial.read()); // empty buffer again
// start message
Serial.println("\nMPU6050 Calibration Sketch");
delay(2000);
Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
delay(3000);
// verify connection
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(1000);
// reset offsets
accelgyro.setXAccelOffset(0);
accelgyro.setYAccelOffset(0);
accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(0);
accelgyro.setYGyroOffset(0);
accelgyro.setZGyroOffset(0);
}
/// LOOP
void loop() {
if (state==0){
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(1000);
}
if (state==1) {
Serial.println("\nCalculating offsets...");
calibration();
state++;
delay(1000);
}
if (state==2) {
meansensors();
Serial.println("\nFINISHED!");
Serial.print("\nSensor readings with offsets:\t");
Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);
Serial.print("Your offsets:\t");
Serial.print(ax_offset);
Serial.print("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
Serial.println(gz_offset);
Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
while (1);
}
}
/// FUNCTIONS
void meansensors(){
long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;
while (i<(buffersize+101)){
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
buff_ax=buff_ax+ax;
buff_ay=buff_ay+ay;
buff_az=buff_az+az;
buff_gx=buff_gx+gx;
buff_gy=buff_gy+gy;
buff_gz=buff_gz+gz;
}
if (i==(buffersize+100)){
mean_ax=buff_ax/buffersize;
mean_ay=buff_ay/buffersize;
mean_az=buff_az/buffersize;
mean_gx=buff_gx/buffersize;
mean_gy=buff_gy/buffersize;
mean_gz=buff_gz/buffersize;
}
i++;
delay(2); //Needed so we don't get repeated measures
}
}
void calibration(){
ax_offset=-mean_ax/8;
ay_offset=-mean_ay/8;
az_offset=(16384-mean_az)/8;
gx_offset=-mean_gx/4;
gy_offset=-mean_gy/4;
gz_offset=-mean_gz/4;
while (1){
int ready=0;
accelgyro.setXAccelOffset(ax_offset);
accelgyro.setYAccelOffset(ay_offset);
accelgyro.setZAccelOffset(az_offset);
accelgyro.setXGyroOffset(gx_offset);
accelgyro.setYGyroOffset(gy_offset);
accelgyro.setZGyroOffset(gz_offset);
meansensors();
Serial.println("...");
if (abs(mean_ax)<=acel_deadzone) ready++;
else ax_offset=ax_offset-mean_ax/acel_deadzone;
if (abs(mean_ay)<=acel_deadzone) ready++;
else ay_offset=ay_offset-mean_ay/acel_deadzone;
if (abs(16384-mean_az)<=acel_deadzone) ready++;
else az_offset=az_offset+(16384-mean_az)/acel_deadzone;
if (abs(mean_gx)<=giro_deadzone) ready++;
else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);
if (abs(mean_gy)<=giro_deadzone) ready++;
else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);
if (abs(mean_gz)<=giro_deadzone) ready++;
else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);
if (ready==6) break;
}
}
草图通过串行监视器输出所需的加速度计和陀螺仪偏移量。您所要做的就是在程序开始时将它们插入初始化代码中，一切顺利！请注意，每个传感器所需的偏移量差异很大，因此您必须针对所使用的每个MPU-6050传感器重复上述校准程序。
到此，MPU-6050的基本校准结束了。对于大多数应用，例如自平衡机器人和四轴飞行器，传感器现在应该已经足够准确。
第4部分： PID控制器
任何机器人中最重要的元素是控制器。特别是对于自平衡机器人，控制程序至关重要，因为它可以解释传感器数据并确定需要移动多少电机才能使机器人保持直立状态。用于稳定系统的最常见控制器是PID控制器。因此，让我们看一下它是如何工作的：
4.1 PID控制器
PID代表比例，积分和微分，是指用于计算输出的数学方程式。
P分量简单地吸收了机器人的当前角度，并使电动机沿与机器人掉落相同的方向运动。因此，机器人掉落到目标的距离越远，电机移动得越快。如果单独使用P组件，机器人可能会稳定一段时间，但系统将趋于超调，振动并最终掉落。
I组件用于累积任何错误。例如，如果机器人倾向于跌落到一侧，则它知道它需要朝相反的方向移动，以保持在目标上并防止向左或向右漂移。
最后，D组件负责衰减任何振荡，并确保机器人不会振动太大。它只是对抗任何运动。
自平衡机器人上使用的PID控制器的示意图。
4.2 实施控制器
现在我们已经了解了基本理论，我们可以开始用代码编写它了。这是基本布局：
// Declare variables
float Kp = 7; // (P)roportional Tuning Parameter
float Ki = 6; // (I)ntegral Tuning Parameter
float Kd = 3; // (D)erivative Tuning Parameter
float iTerm; // Used to accumulate error (integral)
float lastTime = 0; // Records the last time function was called
float maxPID = 255; // The maximum value that can be output
// ================================================================
// === PID CONTROLLER ===
// ================================================================
float PID(float target, float current) {
// Calculate the time since function was last called
float thisTime = millis();
float dT = thisTime - lastTime;
lastTime = thisTime;
// Calculate error between target and current values
float error = target - current;
float pid = 0;
// Calculate the integral and derivative terms
iTerm += error * dT;
float dTerm = (current - oldCurrent) / dT;
// Set old variable to equal new ones
oldCurrent = current;
// Multiply each term by its constant, and add it all up
pid = (error * Kp) + (iTerm * Ki) - (dTerm * Kd);
// Limit PID value to maximum values
if (maxPID > 0 && pid > maxPID) pid = maxPID;
else if (maxPID > 0 && pid < -maxPID) pid = -maxPID;
return pid;
}
这个程序做什么？
首先，该算法使用“ millis（）”函数计算自调用最后一个循环以来的时间。然后计算误差。这是当前值（传感器记录的角度）与目标值（我们要达到的0度角）之间的差。
然后计算PID值并求和，以得出电动机的输出。从总和中减去导数项，因为它旨在抑制任何运动。然后将输出限制为±255，因为这是可以输出到电机的最大PWM值。
尽管该程序几乎是完整的，但我发现只有包含计时功能，我的机器人才能正常工作。该系统可确保定期调用PID控制器功能。在我的自平衡机器人中，我将循环时间设置为10ms（表示每秒100个周期）。这是计时器代码和示例循环功能：
// Any variables for the PID controller go here!
float target = 0;
// Variables for Time Keeper function:
#define STD_LOOP_TIME 10
unsigned long nextTime = 0;
/******** SETUP ************/
void setup() {
// Put all of your setup code here
}
/******* MAIN LOOP *********/
void loop() {
// Only run the controller once the time interval has passed
if (nextTime < millis()) {
nextTime = millis() + STD_LOOP_TIME;
angle = getAngle();
motorOutput = PID(target, angle);
moveMotors(motorOutput);
}
}
/****** PID CONTROLLER *****/
float PID(float target, float current) {
// PID code from above goes in here!
return pid;
}
不幸的是，这还不是故事的结局！尽管PID控制器代码完整，但仍需要适合您自己的机器人的PID常数。这些常数取决于重量，电机速度和机器人的形状等因素，因此它们在机器人之间可能会有很大差异。这是有关如何校准PID值的简要说明：
4.3 校准您的PID控制器
创建某种方式，可以在机器人运行时更改其PID常数。一种选择是使用电位计或其他一些模拟输入，以能够增加或减少PID常数。我个人使用USB连接和串行监视器发送新的PID值。这很重要，因为您可以立即查看新PID值的工作情况，而不必重新上载代码数百次！
将所有PID常数设置为零。与任何其他地方一样，这是一个很好的起点。
缓慢增加P常数值。在执行此操作时，请握住机器人以确保其不会掉落并粉碎成一百万个！你应该增加P不变，直到机器人快速响应任何倾斜，然后只让在其他方向上的机器人的超调。
现在增加I常数。该组件需要正确一点技巧。您应该保持相对较低，因为它会很快累积错误。从理论上讲，机器人应该仅能设置P和I常数就能稳定下来，但会产生很大的振荡并最终跌落。
提升D常数。很多。 导数分量抵抗任何运动，因此有助于抑制任何振荡并减少过冲。我发现必须将此常数设置为明显高于其他两个常数（x10到x100更多）才能产生效果。同样，不要将其设置得太高，因为它会降低机器人对外力（也就是被推到周围）做出反应的能力。
花很多无用的时间来稍微修改PID值。这可能是该过程中最长的部分，因为它没有太多方法。您只需要增加和减少值，直到达到机器人的最佳甜蜜点！
如果您有任何疑问或建议，请在下面发表评论。
第5部分：将所有内容放在一起
到目前为止，我们已经研究了自动平衡机器人背后的所有单个主题。在本教程的最后一部分中，我将把它们结合在一起，并为您提供一些有关设计和组装自己的机器人的准则！
5.1 设计机器人
重量分配：自动平衡机器人以倒立摆的原理工作。这意味着当所有质量都放置在尽可能高的位置时，系统最稳定。这似乎违背了常识。通常，系统的重心较低时会更稳定。在这种情况下，将质量保持在顶部会增加系统的惯性，这意味着机器人有更多时间来响应平衡变化。因此，我的第一个建议是将最重的物体（例如电池）放在机器人的顶部。
传感器位置： 加速度计/陀螺仪模块的位置也很重要。当我今年在都柏林创客博览会上展示我的平衡机器人时，我问了很多人他们认为传感器应该放在哪里。大多数人猜测它应该在最上面，因为这是记录最大运动量的地方！
实际上，我们只想尽量避免这种平移运动，因为我们只对机器人的旋转感兴趣。因此，应将传感器准确放置在两个车轮之间的旋转轴上。将传感器进一步放在框架上会在读数中引入噪声和抖动，并可能导致反馈环路（类似于麦克风离扬声器太近时发出的吱吱声）。
框架设计：对于框架 的其余部分，您想如何处理它取决于您自己的想象。我在下面提供了一些图片和草图，以帮助您提出自己的设计。尽管我3D打印了两个框架，但我还是用棒棒糖棒制作了第一个原型（效果很好）！
5.2 组装框架
将框架和电子设备放在一起实际上是该项目最简单的部分！设计好框架并准备就绪后，只需将所有组件粘在一起/拧在一起即可。这是我为帮助您进行机器人接线而制作的示意图：
 |  |  ) |
5.3 组合程序
在本教程的前面各部分中，我包含了一些代码片段，向您展示自动平衡机器人的各个部分应如何工作。在这里，我将所有部分都编译成了一个代码，您可以为自己的机器人使用和修改这些代码。我添加了很多评论，以使该程序尽可能易于遵循！
注意：此代码针对我正在使用的特定组件进行了编程，例如Arduino电机护罩和MPU6050 Accel-Gyro模块。
/* * * * * * * * * * * * * * * * * * * * * *
* SELF-BALANCING ROBOT
* =========================================
*
* Code by: Simon Bluett
* Email: hello@chillibasket.com
* Website: wired.chillibasket.com
*
* 7/10/15, Version 2.0
*
* Here are some hints when you try to use this code:
*
* > Ensure pin-mapping is correct for your robot (line 54)
* > Ensure calibration values are correct for your sensor (line 181)
* > Uncomment (line 700) in order to see if your sensor is working
* > Play with your PID values on (line 93)
* > Ensure that your left & right motors aren't inverted (line 355)
* > Confirm whether you want the Pitch ypr[1] or Roll ypr[2] sensor readings!
* * * * * * * * * * * * * * * * * * * * * */
/* * * * * * * * * * * * * * * * * * * * * *
* This Demo makes use of the I2Cdev and MPU6050 libraries, and the demonstration
* sketch written by (Jeff Rowberg <jeff@rowberg.net>), modified to work
* with the Intel Galileo Development Board:
* -- -- -- -- -- -- -- -- -- -- -- -- -- --
* 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.
* * * * * * * * * * * * * * * * * * * * * */
// 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_6Axis_MotionApps20.h>
#include <Wire.h>
// Specific I2C addresses may be passed as a parameter here
MPU6050 mpu; // Default: AD0 low = 0x68
// Define the pin-mapping
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define DIR_A 12 // Direction Pin, Motor A
#define DIR_B 13 // Direction Pin, Motor B
#define PWM_A 3 // PWM, Motor A (Left Motor)
#define PWM_B 11 // PWM, Motor B (Right Motor)
#define BRK_A 9 // Brake, Motor A
#define BRK_B 8 // Brake, Motor B
#define BTN_1 10 // On/Off Button
#define BTN_2 7 // Set Centre of Gravity Button
#define LED_1 5 // Low-battery Warning LED
#define LED_2 4 // Current mode LED
// Max PWM parameters
#define MAX_TURN 30
// MPU Control/Status
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
bool dmpReady = false; // Set true if DMP init was successful
uint8_t devStatus; // Return status after device operation (0 = success, !0 = error)
uint8_t mpuIntStatus; // Holds actual interrupt status byte from MPU
uint16_t packetSize; // Expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // Count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// Orientation/Motion
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
Quaternion q; // [w, x, y, z] Quaternion Container
VectorFloat gravity; // [x, y, z] Gravity Vector
int16_t gyro[3]; // [x, y, z] Gyro Vector
float ypr[3]; // [yaw, pitch, roll] Yaw/Pitch/Roll & gravity vector
float averagepitch[50]; // Used for averaging pitch value
// For PID Controller
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float Kp = 8; // (P)roportional Tuning Parameter
float Ki = 2; // (I)ntegral Tuning Parameter
float Kd = 5; // (D)erivative Tuning Parameter
float lastpitch; // Keeps track of error over time
float iTerm; // Used to accumulate error (integral)
float targetAngle = 2.1; // Can be adjusted according to centre of gravity
// You can Turn off YAW control, by setting
// the Tp and Td constants below to 0.
float Tp = 0.6; // Yaw Proportional Tuning Parameter
float Td = 0.1; // Yaw Derivative Tuning Parameter
float targetYaw = 0; // Used to maintain the robot's yaw
float lastYawError = 0;
float PIDGain = 0; // Used for soft start (prevent jerking at initiation)
// Motor Control
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int direction_A = 0; // 0 - Forwards, 1 - Backwards
int direction_B = 0; //
int brake_A = 1; // 1 - On, 0 - Off
int brake_B = 1; //
// Runtime variables
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int modeSelect = 1; // System Mode (0 = off, 1 = normal, 2 = guided)
bool initialised = true; // Is the balancing system on?
char inchar = 0; // Hold any incoming characters
float angular_rate = 0; // Used to make sure rate is ~0 when balance mode is initiated
bool newCalibration = false; // If set TRUE, the target angles are recalibrated
// Variables used for timing control
// Aim is 10ms per cycle (100Hz)
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define STD_LOOP_TIME 9
unsigned long loopStartTime = 0;
unsigned long lastTime; // Time since PID was called last (should be ~10ms)
// 0 = Off, 1 = On
int modes = 0;
// ------------------------------------------------------------------
// INITIAL SETUP
// ------------------------------------------------------------------
void setup() {
Wire.begin();
// Initialize serial communication for debugging
Serial.begin(115200);
// Configure LED for output
pinMode(LED_1, OUTPUT);
pinMode(LED_2, OUTPUT);
digitalWrite(LED_1, LOW);
digitalWrite(LED_2, LOW);
// Set as input, internal pullup for buttons
pinMode(BTN_1, INPUT_PULLUP);
pinMode(BTN_2, INPUT_PULLUP);
// Configure Motor I/O
pinMode(DIR_A, OUTPUT); // Left Motor Direction
pinMode(DIR_B, OUTPUT); // Right Motor Direction
pinMode(BRK_A, OUTPUT); // Left Motor Brake
pinMode(BRK_B, OUTPUT); // Right Motor Brake
// Initialize MPU6050
mpu.initialize();
Serial.println("Testing MPU connection:");
Serial.println(mpu.testConnection() ? "> MPU6050 connection successful" : "> MPU6050 connection failed");
Serial.println("Initialising DMP");
devStatus = mpu.dmpInitialize();
/* * * * * * * * * * * * * * * * * * * *
* IMPORTANT!
* Supply your own MPU6050 offsets here
* Otherwise robot will not balance properly.
* * * * * * * * * * * * * * * * * * * */
mpu.setXGyroOffset(93);
mpu.setYGyroOffset(-15);
mpu.setZGyroOffset(30);
mpu.setXAccelOffset(-2500);
mpu.setYAccelOffset(1783);
mpu.setZAccelOffset(877);
// Make sure it worked (returns 0 if so)
if (devStatus == 0) {
Serial.println("Enabling DMP");
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// Set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println("DMP Ready! Let's Proceed.");
Serial.println("Robot is now ready to balance. Hold the robot steady");
Serial.println("in a vertical position, and the motors should start.");
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// In case of an error with the DMP
if(devStatus == 1) Serial.println("> Initial Memory Load Failed");
else if (devStatus == 2) Serial.println("> DMP Configuration Updates Failed");
}
}
// -------------------------------------------------------------------
// PID CONTROLLER
// -------------------------------------------------------------------
int PID(float pitch) {
// Calculate time since last time PID was called (~10ms)
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
unsigned long thisTime = millis();
float timeChange = float(thisTime - lastTime);
// Calculate Error
float error = targetAngle - pitch;
// Calculate our PID terms
// PID values are multiplied/divided by 10 in order to allow the
// constants to be numbers between 0-10.
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float pTerm = Kp * error * 10;
iTerm += Ki * error * timeChange / 10;
float dTerm = Kd * (pitch - lastpitch) / timeChange * 100;
if (Ki == 0) iTerm = 0;
lastpitch = pitch;
lastTime = thisTime;
// Obtain PID output value
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float PIDValue = pTerm + iTerm - dTerm;
// Set a minimum speed (motors will not move below this - can help to reduce latency)
//if(PIDValue > 0) PIDValue = PIDValue + 10;
//if(PIDValue < 0) PIDValue = PIDValue - 10;
// Limit PID value to maximum PWM values
if (PIDValue > 255) PIDValue = 255;
else if (PIDValue < -255) PIDValue = -255;
return int(PIDValue);
}
// -------------------------------------------------------------------
// YAW CONTROLLER
// -------------------------------------------------------------------
int yawPD(int yawError) {
// Calculate our PD terms
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float pTerm = Tp * yawError;
float dTerm = Kd * (yawError - lastYawError) / 10;
lastYawError = yawError;
// Obtain PD output value
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int yawPDvalue = int(-pTerm + dTerm);
// Limit PD value to maximum
if (yawPDvalue > MAX_TURN) yawPDvalue = MAX_TURN;
else if (yawPDvalue < -MAX_TURN) yawPDvalue = -MAX_TURN;
//Serial.print("Error: ");
//Serial.print(yawError);
//Serial.print(" - PD: ");
//Serial.println(yawPDvalue);
return yawPDvalue;
}
// -------------------------------------------------------------------
// MOVEMENT CONTROLLER
// -------------------------------------------------------------------
// This function calculate the PWM output required to keep the robot
// balanced, to move it back and forth, and to control the yaw.
void MoveControl(int pidValue, float yaw){
// Set both motors to this speed
int left_PWM = pidValue;
int right_PWM = pidValue;
/* YAW CONTROLLER */
// Check if turning left or right is faster
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int leftTurn, rightTurn;
float newYaw = targetYaw;
if((yaw > 0) && (newYaw < 0)){
rightTurn = yaw + abs(newYaw);
leftTurn = (180 - yaw) + (180 - abs(newYaw));
} else if ((yaw < 0) && (newYaw > 0)){
rightTurn = (180 - abs(yaw)) + (180 - newYaw);
leftTurn = abs(yaw) + newYaw;
} else if (((yaw > 0) && (newYaw > 0)) || ((yaw < 0) && (newYaw < 0))){
rightTurn = newYaw - yaw;
if (rightTurn > 0){
leftTurn = rightTurn;
rightTurn = 360 - leftTurn;
} else if (rightTurn < 0){
rightTurn = abs(rightTurn);
leftTurn = 360 - abs(rightTurn);
} else if (rightTurn == 0){
rightTurn = leftTurn = 0;
}
}
// Apply yaw PD controller to motor output
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
if ((leftTurn == 0) && (rightTurn == 0)){
// Do nothing
} else if (leftTurn <= rightTurn){
leftTurn = yawPD(leftTurn);
left_PWM = left_PWM - leftTurn;
right_PWM = right_PWM + leftTurn;
} else if (rightTurn < leftTurn){
rightTurn = yawPD(rightTurn);
left_PWM = left_PWM + rightTurn;
right_PWM = right_PWM - rightTurn;
}
// Limits PID to max motor speed
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
if (left_PWM > 255) left_PWM = 255;
else if (left_PWM < -255) left_PWM = -255;
if (right_PWM > 255) right_PWM = 255;
else if (right_PWM < -255) right_PWM = -255;
// Send command to left motor
if (left_PWM >= 0) Move(0, 0, int(left_PWM)); // '0' = Left-motor, '1' = Right-motor
else Move(0, 1, (int(left_PWM) * -1));
// Send command to right motor
if (right_PWM >= 0) Move(1, 1, int(right_PWM)); // '0' = Forward, '1' = Backward
else Move(1, 0, (int(right_PWM) * -1));
}
// -------------------------------------------------------------------
// MOTOR CONTROLLER
// -------------------------------------------------------------------
void Move(int motor, int direction, int speed) {
// Left Motor
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
if (motor == 0){
// Set motor direction (only if it is currently not that direction)
if (direction == 0){
if (direction_A == 1) digitalWrite(DIR_A, HIGH); // Forwards
direction_A = 0;
} else {
if (direction_A == 0) digitalWrite(DIR_A, LOW); // Backwards
direction_A = 1;
}
// Release brake (only if brake is active)
if (brake_A == 1){
digitalWrite(BRK_A, LOW);
brake_A = 0;
}
// Send PWM data to motor A
analogWrite(PWM_A, speed);
// Right Motor
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
} else if (motor == 1){
// Set motor direction (only if it is currently not that direction)
if (direction == 0){
if (direction_B == 1) digitalWrite(DIR_B, HIGH); // Forwards
direction_B = 0;
} else {
if (direction_B == 0) digitalWrite(DIR_B, LOW); // Backwards
direction_B = 1;
}
// Release brake (only if brake is active)
if (brake_B == 1){
digitalWrite(BRK_B, LOW);
brake_B = 0;
}
// Send PWM data to motor A
analogWrite(PWM_B, speed);
// Stop both motors
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
} else if (motor = 3){
analogWrite(PWM_A, 0);
analogWrite(PWM_B, 0);
digitalWrite(BRK_A, HIGH);
digitalWrite(BRK_B, HIGH);
brake_A = 1;
brake_B = 1;
}
}
// -------------------------------------------------------------------
// READ INPUT FROM SERIAL
// -------------------------------------------------------------------
void readSerial() {
// Initiate all of the variables
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int changestate = 0; // Which action needs to be taken?
int no_before = 0; // Numbers before decimal point
int no_after = 0; // Numbers after decimal point
bool minus = false; // See if number is negative
inchar = Serial.read(); // Read incoming data
if (inchar == 'P') changestate = 1;
else if (inchar == 'I') changestate = 2;
else if (inchar == 'D') changestate = 3;
// Tell robot to calibrate the Centre of Gravity
else if (inchar == 'G') calibrateTargets();
// Records all of the incoming data (format: 00.000)
// And converts the chars into a float number
if (changestate > 0){
if (Serial.available() > 0){
// Is the number negative?
inchar = Serial.read();
if(inchar == '-'){
minus = true;
inchar = Serial.read();
}
no_before = inchar - '0';
if (Serial.available() > 0){
inchar = Serial.read();
if (inchar != '.'){
no_before = (no_before * 10) + (inchar - '0');
if (Serial.available() > 0){
inchar = Serial.read();
}
}
if (inchar == '.'){
inchar = Serial.read();
if (inchar != '0'){
no_after = (inchar - '0') * 100;
}
if (Serial.available() > 0){
inchar = Serial.read();
if (inchar != '0'){
no_after = no_after + ((inchar - '0') * 10);
}
if (Serial.available() > 0){
inchar = Serial.read();
if (inchar != '0'){
no_after = no_after + (inchar - '0');
}
}
}
}
}
// Combine the chars into a single float
float answer = float(no_after) / 1000;
answer = answer + no_before;
if (minus) answer = answer * -1;
// Update the PID constants
if (changestate == 1){
Kp = answer;
Serial.print("P - ");
} else if (changestate == 2){
Ki = answer;
Serial.print("I - ");
} else if (changestate == 3){
Kd = answer;
Serial.print("D - ");
}
Serial.print("Constant Set: ");
Serial.println(answer, 3);
} else {
changestate = 0;
}
}
}
// -------------------------------------------------------------------
// RECALIBRATE TARGET VALUES
// -------------------------------------------------------------------
// Takes a number of readings and gets new values for the target angles.
// Robot must be held upright while this process is being completed.
void calibrateTargets(){
targetAngle = 0;
targetYaw = 0;
for(int calibrator = 0; calibrator < 50; calibrator++){
accelgyroData();
targetAngle += pitch();
targetYaw += yaw();
delay(10);
}
// Set our new value for Pitch and Yaw
targetAngle = targetAngle / 50;
targetYaw = targetYaw / 50;
Serial.print("Target Pitch: ");
Serial.print(targetAngle, 3);
Serial.print(", Target Yaw: ");
Serial.print(targetYaw, 3);
newCalibration = false;
}
// -------------------------------------------------------------------
// GET PITCH AND YAW VALUES
// -------------------------------------------------------------------
// This simply converts the values from the accel-gyro arrays into degrees.
float pitch(){
return (ypr[1] * 180/M_PI);
}
float yaw(){
return (ypr[0] * 180/M_PI);
}
float angRate(){
return -((float)gyro[1]/131.0);
}
// -------------------------------------------------------------------
// GET ACCEL_GYRO DATA
// -------------------------------------------------------------------
void accelgyroData(){
// Reset interrupt flag and get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus();
// Get current FIFO count
fifoCount = mpu.getFIFOCount();
// Check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// Reset so we can continue cleanly
mpu.resetFIFO();
Serial.println("Warning - FIFO Overflowing!");
// otherwise, check for DMP data ready interrupt (this should happen exactly once per loop: 100Hz)
} else if (mpuIntStatus & 0x02) {
// Wait for correct available data length, should be less than 1-2ms, if any!
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// Get sensor data
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGyro(gyro, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
mpu.resetFIFO();
//Serial.print(ypr[1]);
//Serial.print(" - ");
//Serial.println(ypr[0]);
}
}
// -------------------------------------------------------------------
// MAIN PROGRAM LOOP
// -------------------------------------------------------------------
void loop() {
// If the "SET" button is pressed
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
if (digitalRead(BTN_2) == LOW){
digitalWrite(LED_1, HIGH);
calibrateTargets();
lastpitch = 0;
iTerm = 0;
Serial.println("> Setting new centre of gravity <");
delay(250);
mpu.resetFIFO();
digitalWrite(LED_1, LOW);
}
// If the "POWER" button is pressed
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
if (digitalRead(BTN_1) == LOW){
if (modeSelect == 1){
Serial.println("> Turning off balancing system <");
initialised = false;
modeSelect = 0;
Move(3,0,0); // Stop both motors from moving
digitalWrite(LED_2, LOW);
} else if (modeSelect == 0){
Serial.println("> Turning on balancing system <");
initialised = false;
modeSelect = 1;
digitalWrite(LED_2, HIGH);
}
delay(500);
mpu.resetFIFO();
}
// Gather data from MPU6050
accelgyroData();
// If the Balance System is turned on:
if (modeSelect == 1){
if (!initialised){
// Wait until robot is vertical and angular rate is almost zero:
if ((pitch() < targetAngle+0.1) && (pitch() > targetAngle-0.1) && (abs(angRate()) < 0.3)){
Serial.println(">>>> Balancing System Active <<<<");
initialised = true;
lastpitch = pitch();
iTerm = 0;
}
// Otherwise, run the PID controller
} else {
// Stop the system if it has fallen over:
if ((pitch() < -45) || (pitch() > 45)){
// Stop the motors
Move(3, 0, 0);
// Reset runtime variables
lastpitch = 0;
iTerm = 0;
initialised = false;
Serial.println(">>>> Balancing System Stopped <<<<");
} else {
// A bit of function-ception happening here:
//Serial.println(pitch());
MoveControl(PID(pitch()), yaw());
}
}
}
if (Serial.available() > 0){ // If new PID values are being sent by the interface
readSerial(); // Run the read serial method
}
// Call the timing function
// Very important to keep the response time consistent!
timekeeper();
}
// -------------------------------------------------------------------
// TIME KEEPER
// -------------------------------------------------------------------
void timekeeper() {
// Calculate time since loop began
float timeChange = millis() - loopStartTime;
// If the required loop time has not been reached, please wait!
if (timeChange < STD_LOOP_TIME) {
delay(STD_LOOP_TIME - timeChange);
}
// Update loop timer variables
loopStartTime = millis();
}
以下是如何在您的机器人上使用此代码的细分：
- 在启动程序之前，请通过USB将开发板连接至计算机，然后在Arduino软件中打开一个终端窗口（波特率：115200）。
- 等待直到机器人准备就绪：在启动时，机器人会自动初始化MPU6050模块。完成此操作后，将出现以下消息：
DMP Ready! Let’s Proceed. - 设置重心：您应该设置机器人的重心，以便机器人知道向上的方向！为此，请稳定机器人，使其保持竖直，车轮离开地面，然后按下连接到GPIO-4的按钮。GPIO-2上的LED指示灯将闪烁，并出现以下消息：
> Setting new centre of gravity < - 自动开/关：如果机器人跌落或侧卧，则机器人的电动机会自动关闭。要重新打开它们，请将机器人稳定地直立放置。电动机应启动，并显示以下消息：
>>>> Balancing System Active <<<<
如果机器人跌倒并且电动机关闭，则会出现以下消息：
>>>> Balancing System Stopped <<<< - 手动开/关：要手动打开/关闭平衡系统，请按连接到GPIO-7的按钮。如果打开了平衡系统，则GPIO-10上的LED将亮起。将显示以下消息之一，让您知道机器人处于哪种状态：
> Turning off balancing system <
> Turning on balancing system < - 发送新的PID值： 请阅读我的指南PID控制器，以了解如何校准机器人。您可以通过控制台窗口发送新的PID值，方法是键入要设置的常量的字母（P，I或D），然后输入要设置其的数字。然后按输入/返回键发送。该代码接受介于0.01 – 99.99之间的任何数字。例如：
P8.2 I1.51 D15
这将[P]比例常数设置为8.2，[I]积分常数设置为1.51，并且[D]指令常数设置为15。
处理常见错误
我发现可以通过检查以下内容来解决大多数常见错误：
检查引脚映射： 确保GPIO编号（第54-65行）与您在机器人上使用的GPIO编号匹配。
更新MPU-6050偏移： 每个传感器都有唯一的偏移值，必须在上面输入（第183-188行）。我将在本教程的“ 校准和优化MPU6050 ”部分中说明如何找到这些偏移量。
确保传感器正常工作： 通过取消注释（第704行）我的代码来检查MPU-6050是否正常工作。跑步时，机器人应在控制台上显示当前角度。垂直放置时，角度应为0。向前/向后倾斜时，数字应为正/负（以度为单位）。
两个电动机都应沿相同方向旋转： 将（第102-103行）上的旋转常数设置为零。现在，两个电动机都应沿相同的方向旋转。如果不是，则其中一台电动机向后接线。
电机在错误的方向上进行平衡： 电机并没有阻止机器人坠落，而是加速了坠落。这意味着两个电动机都反向连接！
6188
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
