简易PID模块（C++）

最新推荐文章于 2024-08-21 13:16:44 发布

blackfly043

最新推荐文章于 2024-08-21 13:16:44 发布

阅读量531

点赞数 1

分类专栏： pid 文章标签： pid

本文链接：https://blog.csdn.net/weixin_43288132/article/details/107711040

版权

pid 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

简易PID模块

源码为WR学长所写，现发布在CSDN，作为传承，留给学弟使用。

库文件

#ifndef WR_CONTROLLER_H__
#define WR_CONTROLLER_H__



typedef struct
{
  float P;//P参数
  float I;//I参数
  float D;//D参数
  float integral;//积分值
  float err;//期望值与当前值差值
  float err_D;//负的当前值的微分
  float integral_max;//积分环节最大值（用于积分限幅）
  float output_max;//输出最大值（用于输出限幅）
  float dt;//控制周期
  uint8_t integral_lock;//1积分锁定 0积分解锁
  uint8_t integral_clear;//写1积分清零  清零后这个字节自动恢复为0  积分清零优先级大于积分锁定
}s_pid_t;



float wr_pid_controller(s_pid_t *pid);
void wr_pid_controller_param_init(s_pid_t *pid,
                                  float dt,
                                  float P,
                                  float I,
                                  float D,
                                  float integral,
                                  float integral_max,
                                  float output_max,
                                  uint8_t integral_lock );


#endif

C文件

#include "include.h"
float wr_pid_controller(s_pid_t *pid)
{
  float output;
  
  if(pid->integral_lock==0)//积分解锁
  {
    pid->integral+=pid->err*pid->dt;
  }
  pid->integral=float_limit(pid->integral,-pid->integral_max,pid->integral_max);//积分环节限幅
  
  if(pid->integral_clear)//积分清零
  {
    pid->integral=0;
    pid->integral_clear=0;
  }
  
  output=pid->err*pid->P+pid->err_D*pid->D+pid->integral*pid->I;
  output=float_limit(output,-pid->output_max,pid->output_max);//输出限幅
  return output;
}

void wr_pid_controller_param_init(s_pid_t *pid,
                                  float dt,
                                  float P,
                                  float I,
                                  float D,
                                  float integral,
                                  float integral_output_max,//积分环节输出最大值，不是积分最大值，pid->integral_max=integral_output_max/I;
                                  float output_max,
                                  uint8_t integral_lock )
{
  pid->dt=dt;
  pid->P=P;
  pid->I=I;
  pid->D=D;
  pid->integral=integral;
  if(I>0)
  {
    pid->integral_max=integral_output_max/I;
  }
  else 
  {
    pid->integral_max=0;
  }
  pid->output_max=output_max;
  pid->integral_lock=integral_lock;
}

在别的文件中使用

s_PID PID_stand,PID_speed,PID_turn;
s_pid_t s_pid_speed_flat;
const float control_dt=0.005;//5ms控制周期
const float dt=0.005;
void pid_init()
{
  wr_pid_controller_param_init(&s_pid_speed_flat,//*s_pid_t
                                  control_dt,//dt
                                  11.8,//P11.8
                                  0,//I
                                  0,//D
                                  0,//integral
                                  3,//积分环节输出最大值
                                  50,//output_max
                                  0);//integral_lock


}

void speed_control_stand()
{
  float speed_for_control;
  speed_for_control=-(speed_L+speed_R)/2.0f;
  s_pid_speed_stand.err=(s_control_data.speed_want_stand-speed_for_control);
  s_pid_speed_stand.P=linechart_for_speed_P(fabsf(s_pid_speed_stand.err));
  s_control_data.stand_angle_want+=0.9*(wr_pid_controller(&s_pid_speed_stand)-			s_control_data.stand_angle_want);

}


void control_main(){//平跑速度要大于直立速度
 	 static int i;
    pid_init();
    s_control_data.speed_want_stand=2;
    speed_control_stand(s_control_data.speed_want_stand);
   }