PID控制_位置式

位置式：

1. 位置式:

pid.c:

float PID_calculate( float dT_s,            //周期（单位：秒）
                    float in_ff,				//前馈值
                    float expect,				//期望值（设定值）
                    float feedback,			//反馈值（）
                    _PID_arg_st *pid_arg, //PID参数结构体
                    _PID_val_st *pid_val,	//PID数据结构体
                    float inte_d_lim,//积分误差限幅
                    float inte_lim,			//integration limit，积分限幅
										float err_lim     //误差死区
                      )
{
  float hz;
  hz = safe_div(1.0f,dT_s,0);
  pid_val->err = (expect - feedback);

  if( ( pid_val->err <=  err_lim) && ( pid_val->err > 0 ) )   //误差死区
  {
    pid_val->err = 0;
  }
  else if( ( pid_val->err >=  -err_lim) && ( pid_val->err < 0 ) )
  {
    pid_val->err = 0;
  }

  pid_val->exp_d = (expect - pid_val->exp_old) *hz;

  if(pid_arg->fb_d_mode == 0)
  {
    pid_val->fb_d = (feedback - pid_val->feedback_old) *hz;
  }
  else
  {
    pid_val->fb_d = pid_val->fb_d_ex;
  }
  pid_val->differential = (pid_arg->kd_ex *pid_val->exp_d - pid_arg->kd_fb *pid_val->fb_d);

  pid_val->err_i += pid_arg->ki *LIMIT((pid_val->err ),-inte_d_lim,inte_d_lim )*dT_s;//)
  pid_val->err_i = LIMIT(pid_val->err_i,-inte_lim,inte_lim);
  pid_val->out = pid_arg->k_ff *in_ff
    + pid_arg->kp *pid_val->err
      +	pid_val->differential
    	+ pid_val->err_i;
  pid_val->feedback_old = feedback;
  pid_val->exp_old = expect;
  return (pid_val->out);
}

pid.h:

#define LIMIT( x,min,max ) ( ((x) <= (min)) ? (min) : ( ((x) > (max))? (max) : (x) ) )

typedef struct
{
	int exp;
	int fb;
	int error;
	int out;
} _ctrl_st;

typedef struct
{
	uint8_t fb_d_mode;
	float kp;			 //比例系数
	float ki;			 //积分系数
	float kd_ex;		 	 //微分系数
	float kd_fb; //previous_d 微分先行
//	float inc_hz;  //不完全微分低通系数
//	float k_inc_d_norm; //Incomplete 不完全微分 归一（0,1）
	float k_ff;		 //前馈 

}_PID_arg_st;

typedef struct
{
	float err;
	float exp_old;
	float feedback_old;

	float fb_d;
	float fb_d_ex;
	float exp_d;
//	float err_d_lpf;
	float err_i;
	float ff;
	float pre_d;

	float out;
	float differential;
}_PID_val_st;

typedef struct
{
  _ctrl_st      *ctrl_st;
  _PID_arg_st   *arg_st;
  _PID_val_st   *val_st;
}_PID_DATA;

float PID_calculate( float T,            //周期
										float in_ff,				//前馈
										float expect,				//期望值（设定值）
										float feedback,			//反馈值
										_PID_arg_st *pid_arg, //PID参数结构体
										_PID_val_st *pid_val,	//PID数据结构体
										float inte_d_lim,
										float inte_lim,			//integration limit，积分限幅
										float err_lim	   //误差死区
										   );			//输出

使用：

static _PID_arg_st arg_st;//实例化
static _PID_val_st val_st;//实例化
static _ctrl_st    ctrl_st;//实例化
arg_st->kp = 1.2f;
arg_st->ki = 3.2f;
arg_st->kd_fb = 0.05f;