摆球控制程序PID算法

使用STM32控制

用正点原子的STM32F4探索者开发板（大材小用）的作为主控控制摆球

PID控制算法

用PID控制（整体程序写法比较混乱）
基本思路：

1. 小球在平面上分为x轴方向和y轴方向
2. 实现速度环、
3. 先实现在x轴方向给小球任意速度能让小球在x轴方向上的速度为0
4. 再实现在y轴方向给小球任意速度能让小球在y轴方向上的速度为0
5. 实现位置环
6. 实现小球停到任意x轴位置
7. 实现小球停到任意y轴位置
8. 将小球的速度环和位置环串联

调试

1. 速度环一般采用PI控制
2. 位置环一般采用PD控制

下面是源代码

//PID头文件内容
//关于位置环的结构体定义
typedef struct
{
  float SetPlace;    //定义设定值
  float ActualPlace; //定义实际值
  float LastPlace;   //定义实际值
  float err;         //定义偏差值
  float err_last;    //定义上一个偏差值
  float Kp, Ki, Kd;  //定义比例、积分、微分系数
  float result;      //pid计算结果
  float integral;    //定义积分值
} pid_p;
//关于速度环的结构体定义
typedef struct
{
  float SetV;             //定义设定值
  float ActualV;          //定义实际值
  float err_V;            //定义偏差值
  float err_last_V;       //定义上一个偏差值
  float Kp_V, Ki_V, Kd_V; //定义比例、积分、微分系数
  float result_V;         //pid计算结果
  float integral_V;       //定义积分值
} pid_c;
//位置环
void PID_init(void);
float PID_realize(float P, float P_r);
void PID_init_Y(void);
float PID_realize_Y(float P, float P_r);

//速度环
void PID_init_Vx(void);
float PID_Vx_realize(float V, float V_r);
void PID_init_Vy(void);
float PID_Vy_realize(float V, float V_r);


//PID源文件
float last_x=0; //x上一次位置
float last_y=0; //y上一次位置
float x_speed=0;//x轴
float y_speed=0;
void PID_init()//PID参数的初始化
{

  pid.SetPlace = 0;    // 设定的预期位置值
  pid.ActualPlace = 0; // 实际位置值
  pid.err = 0.0;       // 当前次实际与理想的偏差
  pid.err_last = 0.0;  // 上一次的偏差
  pid.LastPlace = 0;   // 上一次的位置
  pid.integral = 0.0;  // 积分值
  pid.Kp = 0.5;        // 比例系数0.1
  pid.Ki = 0;          // 积分系数
  pid.Kd = 0;         // 微分系数
}
float PID_realize(float P, float P_r)
{
  char flag = 1;
  pid.SetPlace = P;                         // 固定位置值传入
  pid.ActualPlace = P_r;                    // 实际位置传入 
  pid.err = pid.SetPlace - pid.ActualPlace; //计算偏差
                                        

    pid.integral += pid.err;//积分求和
    pid.result = pid.Kp * pid.err + pid.Ki * pid.integral + pid.Kd * (pid.err - pid.err_last);  //位置式公式
    pid.err_last = pid.err;//留住上一次误差

    pid.LastPlace = pid.ActualPlace;
    return pid.result;
  }
  return 0;
}
void PID_init_Y()
{

  pid_Y.SetPlace = 0;    // 设定的预期位置值
  pid_Y.ActualPlace = 0; // 实际位置值
  pid_Y.err = 0.001;       // 当前次实际与理想的偏差
  pid_Y.err_last = 0.0;  // 上一次的偏差
  pid_Y.integral = 0.0;  // 积分值
  pid_Y.Kp = 0.1;          // 比例系数0.01
  pid_Y.Ki = 0;          // 积分系数
  pid_Y.Kd = 0;        // 微分系数
}

float PID_realize_Y(float P, float P_r)
{
  char flag = 1;
  pid_Y.SetPlace = P;                             // 固定位置值传入
  pid_Y.ActualPlace = P_r;                        // 实际电压传入 = ADC_Value * 3.3f/ 4096
  pid_Y.err = pid_Y.SetPlace - pid_Y.ActualPlace; //计算偏差                                              

  pid_Y.integral += pid_Y.err;                                                                               //积分求和
  pid_Y.result = pid_Y.Kp * pid_Y.err + pid_Y.Ki * pid_Y.integral + pid_Y.Kd * (pid_Y.err - pid_Y.err_last); //位置式公式
  pid_Y.err_last = pid_Y.err;

    //留住上一次误差
    return (pid_Y.result) / 9000;
}
//x轴方向的速度环
void PID_init_Vx()
{

  pid_v.SetV = 0;         // 设定的预期速度值
  pid_v.ActualV = 0;      // 实际速度值
  pid_v.err_V = 0;      // 当前次实际与理想的速度差
  pid_v.err_last_V = 0.0; // 上一次的偏差
  pid_v.integral_V = 0.0; // 积分值
  pid_v.Kp_V = 0.07;         // 比例系数0.03
  pid_v.Ki_V = 0;         // 积分系数
  pid_v.Kd_V = 0.02;         // 微分系数
}
//
float PID_Vx_realize(float V, float V_r)
{
  char flag = 1;
  pid_v.SetV = V;                                                                                                            // 固定位置值传入
  pid_v.ActualV = V_r;                                                                                                       // 实际电压传入 = ADC_Value * 3.3f/ 4096
  pid_v.err_V = pid_v.SetV - pid_v.ActualV;                                                                                  //计算偏差
  pid_v.integral_V += pid_v.err_V;                                                                                           //积分求和
  pid_v.result_V = pid_v.Kp_V * pid_v.err_V + pid_v.Ki_V * pid_v.integral_V + pid_v.Kd_V * (pid_v.err_V - pid_v.err_last_V); //位置式公式
  pid_v.err_last_V = pid_v.err_V;
  return pid_v.result_V;
}
//y轴方向的速度环
void PID_init_Vy()
{

  pid_v.SetV = 0;         // 设定的预期速度值
  pid_v.ActualV = 0;      // 实际速度值
  pid_v.err_V = 0;      // 当前次实际与理想的速度差
  pid_v.err_last_V = 0.0; // 上一次的偏差
  pid_v.integral_V = 0.0; // 积分值
  pid_v.Kp_V = 0.4;         // 比例系数0.1
  pid_v.Ki_V = 0;         // 积分系数
  pid_v.Kd_V = 0.02;         // 微分系数
}
float PID_Vy_realize(float V, float V_r)
{
  char flag = 1;
  pid_v.SetV = V;                                                                                                            // 固定位置值传入
  pid_v.ActualV = V_r;                                                                                                       // 实际电压传入 = ADC_Value * 3.3f/ 4096
  pid_v.err_V = pid_v.SetV - pid_v.ActualV;                                                                                  //计算偏差
  pid_v.integral_V += pid_v.err_V;                                                                                           //积分求和
  pid_v.result_V = pid_v.Kp_V * pid_v.err_V + pid_v.Ki_V * pid_v.integral_V + pid_v.Kd_V * (pid_v.err_V - pid_v.err_last_V); //位置式公式
  pid_v.err_last_V = pid_v.err_V;

  return pid_v.result_V;
}

注意：

1. 位置参数通过摄像头采集小球确定位置
2. 速度参数是通过摄像头采集到当前的位置与上一帧小球位置的差值再乘上帧数（粗略计算）

//部分控制程序
    temp = 0.4*PID_realize(56,x_data)+0.6*PID_Vx_realize(0, x_speed);//
    temp = Servo_Middle_X - temp;//Servo_Middle_X舵机中值
    if (temp <= Servo_Up_X)
    {
      temp = Servo_Up_X;
    }
    else if (temp >= Servo_Down_X)
    {
      temp = Servo_Down_X;
    }
    Servo_Control_1(temp);//控制舵机的函数也就是产生对应的PWM

链接: BiliBili视频最后10秒是比较好的效果.