关于平衡小车速度环系数整定

ILLUSION

我用的板子是UNO、陀螺仪是串口直接输出角度的MPU6050（gy250)、电机驱动用的普通L298n、电机也是垃圾的香蕉电机、测速模块是光电对射测码盘的。

ILLUSION

下面贴上代码并附上直立换调试后的效果
#include <L298.h>
#include <Wire.h>
#include <PID_v1.h>

//电机控制
L298 motor(5,4,6,7);
double output,output_left,output_right;

//角度读取
float YPR[3];
unsigned char Re_buf[8],counter=0;
unsigned char sign=0;

//速度位置控制
float Ksp = 20,Ksi = 0.1;                                             //速度环 ki kp
float shudu_weizhi_out;
int EncoderImpulse_left,EncoderImpulse_right;              //左右编码器脉冲数
int v_left,v_right,weizhi;                                             //左右速度、小车位置

//直立控制
double angle_set=-0.8,angle_input,angle_output,angle_kp=60,angle_ki=0,angle_kd=10.5;   //设定平衡角度，KP\ki\kd值
PID myPID(&angle_input, &angle_output, &angle_set, angle_kp, angle_ki,angle_kd, DIRECT);

//转向控制
//未完成


void setup()
{
  Serial.begin(115200);                                           //初始化
  pinMode(4,INPUT);
  pinMode(7,INPUT);
  attachInterrupt(0,impulse_left, FALLING);               //中断
  attachInterrupt(1,impulse_right, FALLING);
     
  Serial.write(0XA5);
  Serial.write(0X52);                                                //初始化GY25,连续输出模式

  //myPID.SetSetpoint(angle_set);
  //myPID.SetTunings(angle_kp,angle_ki, angle_kd);
  myPID.SetSampleTime(10);
  myPID.SetOutputLimits(-255,255);
  myPID.SetMode(AUTOMATIC);
}
//-------------------------------------------------------------
void loop()
{
  if(sign)
  {  
     sign=0;                                                           
     angle_input =YPR[2];                                                   // 输入赋值，即为测得当前角度
     myPID.Compute();
     speed_clc();
     shudu_weizhi_out = (v_left+v_right)/2*Ksp + weizhi*Ksi;
     output=angle_output+shudu_weizhi_out;
     output_left=output;
     output_right=output;
     if(abs(angle_input)<15)                                                 //角度大于15度即停止输出
     {
            motor.MotorA(output_left);
            motor.MotorB(output_right);
      }
      else
      {
            motor.MotorA(0);
            motor.MotorB(0);
      }

    Serial.print(angle_input);             // 计算时间
    Serial.print(",");
    Serial.print(v_left);                     // 计算时间
    Serial.print(",");
    Serial.print(v_right);                   // 计算时间
    Serial.print(",");
    Serial.print(angle_output);          // 计算时间
    Serial.print(",");
    Serial.print(shudu_weizhi_out);    // 计算时间
    Serial.print(",");
    Serial.println(output);    // PID输出值

        // 记录本次时间
    }
}

void serialEvent() {                                                           //MPU6050代码
  while (Serial.available()) {   
    Re_buf[counter]=(unsigned char)Serial.read();
    if(counter==0&&Re_buf[0]!=0xAA) return;                     // 检查帧头         
    counter++;      
    if(counter==8)                                                            //接收到数据
    {   
       counter=0;                                                               //重新赋值，准备下一帧数据的接收
       if(Re_buf[0]==0xAA && Re_buf[7]==0x55)                  //检查帧头，帧尾
       {                  
            YPR[0]=(Re_buf[1]<<8|Re_buf[2])/100.0;              //合成数据，去掉小数点后2位
            YPR[1]=(Re_buf[3]<<8|Re_buf[4])/100.0;
            YPR[2]=(Re_buf[5]<<8|Re_buf[6])/100.0;         
       }
       sign=1;
    }      
  }
}


void impulse_left(void)                                                                      //测左轮脉冲数
{
  if(digitalRead(4)==0)EncoderImpulse_left =EncoderImpulse_left-1;      //读取数字口4电平状态，若是低电平，代表轮子正传。脉冲数自加
  else EncoderImpulse_left = EncoderImpulse_left+1;                           //若是高电平，代表轮子反转。脉冲数自减
}

void impulse_right(void)                                                                     //测右轮脉冲数
{
  if(digitalRead(7)==1)EncoderImpulse_right = EncoderImpulse_right-1;   //读取数字口4电平状态，若是低电平，代表轮子正传。脉冲数自加
else EncoderImpulse_right = EncoderImpulse_right+1;                           //若是高电平，代表轮子反转。脉冲数自减
}


void speed_clc(void)
{
  static int speed_time_old,EncoderImpulse_left_old,EncoderImpulse_right_old;
  if(millis() - speed_time_old >= 100)
  {
       v_left  = EncoderImpulse_left  -  EncoderImpulse_left_old;
       v_right = EncoderImpulse_right -  EncoderImpulse_right_old;     
       weizhi =( EncoderImpulse_left+EncoderImpulse_right)/2;
       //这个地方可以对上面的俩数度和位置滤波
       EncoderImpulse_left_old  = EncoderImpulse_left;
       EncoderImpulse_right_old = EncoderImpulse_right;
       speed_time_old =millis();
  }
}

ILLUSION

速度还一直整定的不好，不像别人的那样平衡的时候，推一下会自动调节，并且回到原来位置。请大神帮忙看看代码有什么不对的地方

直立视频地址 http://v.youku.com/v_show/id_XMTc2ODM2NzIwNA==.html[/media]