平衡小车调了速度环还是往前跑怎么肥四

Z一叶孤舟 · 发表于 2018-7-7 21:23:32

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
  //=============ËÙ¶&Egrave

I¿ØÖÆÆ÷=======================//
  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;
}