把mstimer去掉后只用原始的内循环，做单角度平衡，实战可见小车确实能在小角度偏差下实现自我纠偏。

角度环代码：

#include <PinChangeInterrupt.h>
#include <MPU6050_tockn.h>
#include <Wire.h>

int STBY = 8; //使能端
int PWMA = 9; //右电机PWM输出控制脚   
int AIN1 = 7; //右电机正极  
int AIN2 = 6; //右电机负极  
int PWMB = 10; //左电机PWM输出控制脚
int BIN1 = 13; //左电机正极 
int BIN2 = 12; //左电机负极  
#define PinA_left 4  //中断 
#define PinA_right 2 //中断1
int count_right = 0,count_left = 0;
float leftSpeed = 0,rightSpeed = 0,speed = 0,angleX = 0;

float angleKp = 20,angleKd = 0.02 ,anglePreError=0,angleError=0,angleOutput=0;     //需要调整的角度环参数
unsigned long now=0,past=0,interval=100;                                         //小车调整姿态的时长,1000为一秒
int zeroAngle = -2;                                                             //小车初始平衡角度,需要微调
MPU6050 mpu6050(Wire);

void setup(){  
  pinMode(STBY, OUTPUT);  
  pinMode(PWMA, OUTPUT);  
  pinMode(AIN1, OUTPUT);  
  pinMode(AIN2, OUTPUT);  
  pinMode(PWMB, OUTPUT);  
  pinMode(BIN1, OUTPUT);  
  pinMode(BIN2, OUTPUT);  
  pinMode(PinA_left, INPUT);  //测速码盘输入
  pinMode(PinA_right, INPUT);
  attachPinChangeInterrupt(digitalPinToPCINT(PinA_left), Code_left, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PinA_right), Code_right, CHANGE);
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  //MsTimer2::set(2000,getAngleX);
  //MsTimer2::start();
}

void loop(){
  now = millis();
  if(now-past>interval){
    getAngleX();
    getSpeed();
    angleOutput = calcAngleOutput(zeroAngle,angleX);
    Serial.println(angleOutput);
    adjustPosture(angleOutput);
    past = now;
  }
}

void runset(int motor, int speed, int direction){
  digitalWrite(STBY, 1); 
  if(motor==1 && direction== 1){
    digitalWrite(AIN1,1);
    digitalWrite(AIN2,0);
    analogWrite(PWMA, speed); 
  }
  if(motor==2 && direction== 1){
    digitalWrite(BIN1,1);
    digitalWrite(BIN2,0);
    analogWrite(PWMB, speed); 
  }
    if(motor==1 && direction== 0){
    digitalWrite(AIN1,0);
    digitalWrite(AIN2,1);
    analogWrite(PWMA, speed); 
  }
  if(motor==2 && direction== 0){
    digitalWrite(BIN1,0);
    digitalWrite(BIN2,1);
    analogWrite(PWMB, speed); 
  }
}

void stop(){  
  digitalWrite(STBY, LOW);   
}  

void Code_left() {
  count_left ++;
} //左测速码盘计数

void Code_right() {
  count_right ++;
} //右测速码盘计数

void getSpeed()
{
  if (digitalRead(AIN1)==1 && digitalRead(AIN2)==0){
    rightSpeed = count_right;
  }
  else if (digitalRead(AIN1)==0 && digitalRead(AIN2)==1){
    rightSpeed = -count_right;
  }
  if (digitalRead(BIN1)==1 && digitalRead(BIN2)==0){
    leftSpeed = count_left;
  }
  else if (digitalRead(BIN1)==0 && digitalRead(BIN2)==1){
    leftSpeed = -count_left;
  }

  count_left = 0;
  count_right = 0;
  //Serial.print(leftSpeed);
  //Serial.print('\t');
  //Serial.println(rightSpeed);
  speed = (leftSpeed+rightSpeed)/2;
}

void getAngleX(){
  mpu6050.update();
  angleX = mpu6050.getAngleX();
}

float calcAngleOutput(float target,float pt){
  angleError = target-pt;
  angleOutput = angleKp*angleError + angleKd*(angleError-anglePreError);
  anglePreError = angleError;
  return angleOutput;
}

void adjustPosture(float angleOutput){
  if (angleOutput>0){
    angleOutput = (angleOutput>254)?254:angleOutput;
    runset(1, (int)abs(angleOutput), 1);
    runset(2, (int)abs(angleOutput), 1);
    }
  else{
    angleOutput = (angleOutput<-254)?-254:angleOutput;
    runset(1, (int)abs(angleOutput), 0);
    runset(2, (int)abs(angleOutput), 0);
  }
}