stm32两轮平衡小车速度环

小车直立环很稳定 加上速度环的p都是可以让小车静止不到  但是 加上速度环 I后 车就不能静止  车会自己向单方向积分  单方向倒

#include "pid.h"
#include "pi.h"
#include "sys.h"
#include "stm32f10x_tim.h"
#include "led.h"
#include "usart.h"   
#include "mpu6050.h"
#include "timer.h"
#include "inv_mpu.h"
#include "delay.h"

int bianma_zuo, bianma_you;
float Velocity,Encoder_Least,Encoder,Movement;
float Encoder_Integral;
extern float Pitch,Roll,Yaw;
   int Moto1,Moto2;   
int  countl,countr;
float speed_sudu;
float  speed;
float zuh;
float qianhou;//?????°??μ?á?
  float bias, kp=60,   kd=0.249;   //96
    float ks=-55,ki=0.01;      //-30   -50   -60/
    float pid_out;
void  balance()
{
   
    bias=Pitch-0;//x?áμ????è±??ˉμ??????±á￠??2?
    pid_out=kp*bias+gyro[1]*kd+ks*speed+zuh*ki;   //?????±á￠pwm
  
    Moto1=pid_out;
    Moto2=pid_out;
    jis(Moto1,Moto2);         
}

void jis(float zuo_pwm,float you_pwm)
{
     
      if(Pitch<-60||Pitch>60)
          {
            zuo_pwm=0;  you_pwm=0;
              IN1=0; IN2=0; IN3=0; IN4=0;
          }
            if(zuo_pwm>0)
              {
                zuof();
                  zuo_pwm=zuo_pwm;
              }
   
      else
              {
                zuoz();
                   zuo_pwm=-zuo_pwm;
              }
     if(zuo_pwm>1000)  
        {
             zuo_pwm=1000;
            }            
    if(you_pwm>0)
       {
         youf();
         you_pwm=you_pwm;         
       }
      else
          {
            youz();
             you_pwm=-you_pwm;
          }
        if(you_pwm>1000)
        { you_pwm=1000;  }
        /*if(zuo_pwm==600)
        {
        zuoz();
        }
            if(you_pwm==600)[
        {
        youz();
        }*/
    /*    TIM_SetCompare1(TIM1,zuo_pwm+290);   
      TIM_SetCompare3(TIM1,you_pwm+270);*/
        TIM_SetCompare1(TIM1,zuo_pwm+290);    //300  290
      TIM_SetCompare3(TIM1,you_pwm+235);  //235
    }
void Psn_Calcu(void)  // ò??×μíí¨??2¨
{
    speed_sudu =(countl+ countr)*0.5;  //?ù?è??2?
    speed *= 0.7;         //         
    speed += speed_sudu*0.3;
  zuh += speed;    //?ù?è??2??y·?
    //zuh += qianhou;//μ??aò??2
    if(zuh>80000)
        zuh=80000;
   
    if(zuh<-80000)
        zuh=-80000;
      
}