PID控制算法

位式PD算法

1. 公式
   
2. 代码：

float PD_control(const float *P_value,const float *D_value,const float *set_value,const float *mea_value)
{
	static float current_error=0;//定义当前误差
	static float last_error=0;//定义上一次误差
	float u=0;//定义输出量
	current_error=*set_value-*mea_value；//求出当前偏差
	u=(*P_value)*current_error+（*D_value）*（current_error-last_error）；//求出输出量U
	last_error=current_error；//将上一次偏差值进行更新
	return u;//返回u值
}

增量式PID算法

1. 公式（此公式图片来源于网络）
   
2. 代码

float PID_control(const float *P,const float *I,const float *D,const float *set_Value,const float *mea_value)
{
	static float current_error=0;
	static float last_error=0;
	static float last_last_error=0;
	float da_U=0;
	static float U=0;
	current_error=*set_Value-*mea_value;
	da_U=(*P)*(current_error-last_error)+(*I)*(current_error+last_error+last_last_error)+(*D)*(current_error-2*(last_error)+last_last_error);
	U=U+da_U;
	last_last_error=last_error;
	last_error=current_error;
	return U;
}