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