想实现平衡小车旋转一定角度后停止，要怎么做，弄了好久小车转向后都不停下来
```c
MPU6050 mpu6050; //实例化一个MPU6050对象，对象名称为mpu6050
int16_t ax, ay, az, gx, gy, gz; //定义三轴加速度，三轴陀螺仪的变量
//TB6612引脚定义
const int right_R1=8;
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;
///////////////////////超声波参数//////////////////////////////
#define echoPin A1 // Echo Pin
#define trigPin A0 // Trigger Pin
long duration , distance; // Duration used to calculate distance
int val;
///////////////////////角度参数//////////////////////////////
float angle_X; //由加速度计算关于X轴的倾斜角度变量
float angle_Y; //由加速度计算关于Y轴的倾斜角度变量
float angle0 = 1; //实际测量出的角度（理想时是0度）
float Gyro_x,Gyro_y,Gyro_z; //用于陀螺仪算出的各轴角速度
///////////////////////角度参数//////////////////////////////
///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001; //陀螺仪噪声的协方差
float Q_gyro = 0.003; //陀螺仪漂移噪声的协方差
float R_angle = 0.5; //加速度计的协方差
char C_0 = 1;
float dt = 0.005; //dt的取值为滤波器采样时间
float K1 = 0.05; // 含有卡尔曼增益的函数，用于计算最优估计值的偏差
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias; //陀螺仪漂移
float accelz = 0;
float angle;
float angleY_one;
float angle_speed;
float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////
//////////////////////PID参数///////////////////////////////
double kp = 34, ki = 0, kd = 0.62; //角度环参数
double kp_speed = 3.6, ki_speed = 0.080, kd_speed = 0; // 速度环参数
double kp_turn = 24, ki_turn = 0, kd_turn = 0.08; // 转向环参数
double setp0 = 0; //角度平衡点
int PD_pwm; //角度输出
float pwm1=0,pwm2=0;
//////////////////中断测速计数/////////////////////////////
#define PinA_left 5 //外部中断
#define PinA_right 4 //外部中断
volatile long count_right = 0;//用于计算霍尔编码器计算的脉冲值(volatile long类型是为了确保数值有效）
volatile long count_left = 0;
int speedcc = 0;
//////////////////////脉冲计算/////////////////////////
int lz = 0;
int rz = 0;
int rpluse = 0;
int lpluse = 0;
int pulseright,pulseleft;
////////////////////////////////PI变量参数//////////////////////////
float speeds_filterold=0;
float positions=0;
int flag1;
double PI_pwm;
int cc;
int speedout;
float speeds_filter;
//////////////////////////////转向PD///////////////////
int turnmax,turnmin,turnout;
float Turn_pwm = 0;
int zz=0;
int turncc=0;
//蓝牙//
int front = 0;//前进变量
int back = 0;//后退变量
int left = 0;//左转标志
int right = 0;//右转标志
int value;
void setup()
{
//设置控制电机的引脚为输出状态
pinMode(right_R1,OUTPUT);
pinMode(right_R2,OUTPUT);
pinMode(left_L1,OUTPUT);
pinMode(left_L2,OUTPUT);
pinMode(PWM_R,OUTPUT);
pinMode(PWM_L,OUTPUT);
//赋初始状态值
digitalWrite(right_R1,1);
digitalWrite(right_R2,0);
digitalWrite(left_L1,0);
digitalWrite(left_L2,1);
analogWrite(PWM_R,0);
analogWrite(PWM_L,0);
pinMode(PinA_left, INPUT); //测速码盘输入
pinMode(PinA_right, INPUT);
pinMode(trigPin, OUTPUT); //超声波输入输出
pinMode(echoPin, INPUT);
// 加入I2C总线
Wire.begin(); //加入 I2C 总线序列
Serial.begin(9600); //开启串口，设置波特率为9600
delay(1500);
mpu6050.initialize(); //初始化MPU6050
delay(2);
//5ms定时中断设置是使用timer2 (注意：使用timer2会对pin3 pin11的PWM输出有影响.)
MsTimer2::set(5, DSzhongduan); //5ms执行一次DSzhongduan这个函数
MsTimer2::start(); //开启中断
}
