代码中使用了两套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();//串口发送数据
}
