代碼中使用了兩套PID參數進行控制，Kp，Ki，Kd是控制平衡速度的。也就是小車往前傾，加速；后傾，減速這個過程。
但是僅靠一個PID控制會出現一個問題：因為小車的平衡角度Setpoint是手動設定的，所以會有一個微小的誤差。當小車以Setpoint為基準進行調整時，可能有一個細微的傾斜，從而導致小車一直向一邊跑。于是引入另一套速度的PID參數：Sp,Si,Sd。 Inputs 為一個變量speedcount，每次中斷檢查車輪的轉向，正向+1，反向-1。這樣，當小車持續向一個方向移動時，speedcount會越來越大，從而產生的Outputs也更大。于是對車輪加一個反向的電壓，使小車盡量停在原地。實現的時候，把Outputs的作用加到第一套PID的目標平衡角度Setpoint上，就可以實現效果。
void setup()
{
delay(50);
pinMode(AN1, OUTPUT);
pinMode(AN2, OUTPUT);
pinMode(BN1, OUTPUT);
pinMode(BN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(STBY, OUTPUT);//放在setup()函數中來確定引腳的功能
pinMode(13, OUTPUT);
digitalWrite(STBY, HIGH);
analogWrite(PWMA, 0); //產生一個PWM為占空比為0的方波
analogWrite(PWMB, 0);
GetParamers();//輸入參數
speedcount = sc;//每次中斷檢查車輪的轉向，正向+1，反向-1。這樣，當小車持續向一個方向移動時，speedcount會越來越大，從而產生的Outputs也更大。于是對車輪加一個反向的電壓，使小車盡量停在原地
Offset_Setpoint = osp;//把Outputs的作用加到目標平衡角度Setpoint上，就可以實現效果。
Serial.begin(115200);//設置波特率為115200
Wire.begin(); //初始化wire,
delay(100);
mpu.initialize();//初始化mpu
delay(2);
devStatus = mpu.dmpInitialize();//初始化mpu6050的數據處理器DMP
//確保DMP工作正常(如果是，返回0)
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
// // 啟用Arduino中斷檢測
attachInterrupt(0, dmpDataReady, RISING);//開啟INT0中斷, mpu6050INT引腳需要接到arduino的PIN 2
mpuIntStatus = mpu.getIntStatus();
dmpReady = true; // 設置DMP就緒標志，以便main loop()函數知道可以使用它
packetSize = mpu.dmpGetFIFOPacketSize(); // 獲得期望的DMP包大小以便稍后比較
}
Setpoint = Offset_Setpoint;
myPID.SetTunings(kp, ki, kd);//設置pid的控制參數
myPID.SetOutputLimits(-255 + dl, 255 - dl);//控制限制量的輸出范圍，PWM限幅
myPID.SetSampleTime(5);//更新新的采樣時間，同時按照比例更新ID參數
myPID.SetMode(AUTOMATIC);//自動初始化
sPID.SetTunings(sp, si, sd);
sPID.SetOutputLimits(-10, 70);
sPID.SetSampleTime(200);
sPID.SetMode(AUTOMATIC);
attachInterrupt(1, speed, RISING);//設置中斷
//MsTimer2::set(500, flash);
// MsTimer2::start();
}
void loop()
{
// 如果編程失敗了，就停在這里
if (!dmpReady)
return;
// 等待MPU中斷或額外數據包可用
if (!mpuInterrupt && fifoCount < packetSize)
return;
// 重置中斷標志并獲得INT_STATUS字節
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
// 獲取當前FIFO計數
mpu.resetFIFO();
// 檢查是否溢出(除非代碼效率太低，否則不會發生)
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);//從DMP中取出Yaw、Pitch、Roll三個軸的角度，放入數組ypr。單位：弧度
angle = -ypr[2] * 180 / M_PI;//提取Roll，轉化為角度制
}
//
Inputs = speedcount;//車輪的轉向
sPID.Compute1();//速度pid的控制計算
Setpoint = Offset_Setpoint + Outputs;//The value we want to Input to maintain (double)
Input = angle; //車輪的角度
Serial.println(Input);//上傳波形到電腦
myPID.Compute2();//角度pid的控制計算
if (angle > 60 || angle < -60)
Output = 0;
motor(Output); //flag=1，轉彎
SerialControl(); //串行發送接收處理
sPID.SetTunings(sp, si, sd);//速度
myPID.SetTunings(kp, ki, kd);//角度
SendData();//串口發送數據
}
