但是仅靠一个PID控制会出现一个问题：因为小车的平衡角度Setpoint是手动设定的，所以会有一个微小的误差。当小车以Setpoint为基准进行调整时，可能有一个细微的倾斜，从而导致小车一直向一边跑。建议引入另一套速度的PID参数：Sp,Si,Sd。
MPU6050接线
SCL -- A5
SDA -- A4
INT -- PIN 2
VCC -- 3.3V
GND -- GND
*******************************************/
#include <PID_v1.h>
#include <LMotorController.h> //L298N控制库
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 10//20
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; //如果DMP初始化成功，设置为true
uint8_t mpuIntStatus; // 保存来自MPU的实际中断状态字节
uint8_t devStatus; // 每次设备操作后返回状态 (0 = success, !0 = error)
uint16_t packetSize; //预期DMP包大小(默认为42字节)
uint16_t fifoCount; // 当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO存储缓冲区
// 定向运动/motion vars
Quaternion q; // [w, x, y, z] 四维
VectorFloat gravity; // [x, y, z] 重力三维矢量
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll 和重力矢量
//PID
double originalSetpoint = 183; //173, 调节小车的稳定中值, 180为默认值, 根据小车实际情况调试
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
//adjust these values to fit your own design
//PID参数整定需要经过多次尝试, 首先调试P, 后D, 最后I
double Kp = 65;
double Kd = 1.2;
double Ki = 90;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
//电机速度偏差调整
double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.6;
//MOTOR CONTROLLER, L298N引脚定义
int ENA = 10;
int IN1 = 13;
int IN2 = 12;
int IN3 = 6;
int IN4 = 7;
int ENB = 9;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
volatile bool mpuInterrupt = false; // 指示MPU中断引脚是否过高
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
// 加入I2C总线(I2Cdev库不会自动这样做)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
delay(1000);
Serial.println("Start!");
mpu.initialize(); //初始化MPU6050
devStatus = mpu.dmpInitialize();
// 校正mpu6050模块
mpu.setXGyroOffset(165);
mpu.setYGyroOffset(-14);
mpu.setZGyroOffset(5);
mpu.setZAccelOffset(528); // 出厂默认
// 确保它工作正常(如果是，返回0)
if (devStatus == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// 启用Arduino中断检测
attachInterrupt(0, dmpDataReady, RISING); //开启INT0中断, mpu6050INT引脚需要接到arduino的PIN 2
mpuIntStatus = mpu.getIntStatus();
// 设置DMP就绪标志，以便main loop()函数知道可以使用它
dmpReady = true;
// 获得期望的DMP包大小以便稍后比较
packetSize = mpu.dmpGetFIFOPacketSize();
//setup PID
//设置PID调节
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(5); //调节间隔为5ms
pid.SetOutputLimits(-250, 250); //PWM限速
}
else
{
// 错误！
// 1 = 初始内存加载失败
// 2 = DMP配置更新失败
// (如果它要中断，通常代码是1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
// 如果编程失败了，就停在这里
if (!dmpReady) return;
// 等待MPU中断或额外数据包可用
while (!mpuInterrupt && fifoCount < packetSize)
{
//没有mpu数据-执行PID计算和输出到电机
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
Serial.print(output);
Serial.print(",");
}
// 重置中断标志并获得INT_STATUS字节
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// 获取当前FIFO计数
fifoCount = mpu.getFIFOCount();
// 检查是否溢出(除非代码效率太低，否则不会发生)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
//重置
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// 否则，检查DMP数据就绪中断(经常发生)
}
else if (mpuIntStatus & 0x02)
{
// 等待正确的可用数据长度
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// 从FIFO读取数据包
mpu.getFIFOBytes(fifoBuffer, packetSize);
// 跟踪FIFO计数在这里，以防有多余1个包可用
// (可以帮助我们立即阅读更多内容而无需等待中断)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180 / M_PI + 180;
}
