但是僅靠一個PID控制會出現(xiàn)一個問題：因為小車的平衡角度Setpoint是手動設(shè)定的，所以會有一個微小的誤差。當小車以Setpoint為基準進行調(diào)整時，可能有一個細微的傾斜甸箱，從而導致小車一直向一邊跑。建議引入另一套速度的PID參數(shù)：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初始化成功芍殖，設(shè)置為true
uint8_t mpuIntStatus; // 保存來自MPU的實際中斷狀態(tài)字節(jié)
uint8_t devStatus; // 每次設(shè)備操作后返回狀態(tài) (0 = success, !0 = error)
uint16_t packetSize; //預期DMP包大小(默認為42字節(jié))
uint16_t fifoCount; // 當前FIFO中所有字節(jié)的計數(shù)
uint8_t fifoBuffer[64]; // FIFO存儲緩沖區(qū)
// 定向運動/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, 調(diào)節(jié)小車的穩(wěn)定中值, 180為默認值, 根據(jù)小車實際情況調(diào)試
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
//adjust these values to fit your own design
//PID參數(shù)整定需要經(jīng)過多次嘗試, 首先調(diào)試P, 后D, 最后I
double Kp = 65;
double Kd = 1.2;
double Ki = 90;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
//電機速度偏差調(diào)整
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();
// 設(shè)置DMP就緒標志谴蔑，以便main loop()函數(shù)知道可以使用它
dmpReady = true;
// 獲得期望的DMP包大小以便稍后比較
packetSize = mpu.dmpGetFIFOPacketSize();
//setup PID
//設(shè)置PID調(diào)節(jié)
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(5); //調(diào)節(jié)間隔為5ms
pid.SetOutputLimits(-250, 250); //PWM限速
}
else
{
// 錯誤豌骏！
// 1 = 初始內(nèi)存加載失敗
// 2 = DMP配置更新失敗
// (如果它要中斷，通常代碼是1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
// 如果編程失敗了隐锭，就停在這里
if (!dmpReady) return;
// 等待MPU中斷或額外數(shù)據(jù)包可用
while (!mpuInterrupt && fifoCount < packetSize)
{
//沒有mpu數(shù)據(jù)-執(zhí)行PID計算和輸出到電機
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
Serial.print(output);
Serial.print(",");
}
// 重置中斷標志并獲得INT_STATUS字節(jié)
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// 獲取當前FIFO計數(shù)
fifoCount = mpu.getFIFOCount();
// 檢查是否溢出(除非代碼效率太低窃躲，否則不會發(fā)生)
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
//重置
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// 否則，檢查DMP數(shù)據(jù)就緒中斷(經(jīng)常發(fā)生)
}
else if (mpuIntStatus & 0x02)
{
// 等待正確的可用數(shù)據(jù)長度
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// 從FIFO讀取數(shù)據(jù)包
mpu.getFIFOBytes(fifoBuffer, packetSize);
// 跟蹤FIFO計數(shù)在這里钦睡，以防有多余1個包可用
// (可以幫助我們立即閱讀更多內(nèi)容而無需等待中斷)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180 / M_PI + 180;
}
