int balance(float Angle,float Gyro)
{
  float Bias,kp=650,kd=3;//700 3.5  400 1  500 2.2    520  2.7  //700  3.3
  int balance;
  Bias=Angle-0;      
  balance=kp*Bias+Gyro*kd;   
  return balance;
}
/**************************************************************************

**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{  
  static float Velocity,Encoder_Least,Encoder;
  static float Encoder_Integral;
  float kp=-130,ki=kp/200;   //-110  
  //=============ËÙ¶&EgraveI¿ØÖÆÆ÷=======================//
  Encoder_Least =(Encoder_Left+Encoder_Right)-0;                  
  Encoder *= 0.5f;                                                   
  Encoder += Encoder_Least*0.5f;                                 
  Encoder_Integral +=Encoder;                                       
  //  Encoder_Integral=Encoder_Integral-Movement;                     
  //  if(Encoder_Integral>10000)   Encoder_Integral=10000;           
  //  if(Encoder_Integral<-10000) Encoder_Integral=-10000;            
  Velocity=Encoder*kp+Encoder_Integral*ki;                        
//  if(Turn_Off(Angle_Balance)==1)   Encoder_Integral=0;      
if(Flag_Target==1)   Encoder_Integral=0;
  return Velocity;
}
/**************************************************************************
**************************************************************************/
void Set_Pwm(int moto1,int moto2)
{
  int siqu=800;//800 4000
  if(moto1<0)   AIN2=1,   AIN1=0;
  else            AIN2=0,   AIN1=1;
  PWMA=myabs(moto1)+siqu;
  if(moto2<0) BIN1=0,   BIN2=1;
  else        BIN1=1,   BIN2=0;
  PWMB=myabs(moto2)+siqu;
}
/**************************************************************************
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=8000;  
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude)  Moto1=Amplitude;
if(Moto2<-Amplitude) Moto2=-Amplitude;
if(Moto2>Amplitude)  Moto2=Amplitude;   
}