PID代码实现

自己写FOC算法中的PID实现部分，关于PID的工作原理，网络上有很多资料这里不再赘述，只贴出实现代码

1、结构体定义

struct Pid_Control
{
		int32_t Error;				//当前误差
		int32_t PreError;			//前一次次的误差
		int32_t LastError;		    //上上的误差

		int16_t KP;						//比例项参数
		int16_t KI;						//积分项参数
		int16_t KD;						//微分项参数
	
		int32_t I_AllErr;			//误差累积
		int32_t P_Parten;			//比例项计算值
		int32_t I_Parten;			//积分项计算值
		int32_t D_Parten;			//微分项计算值
	
		int32_t Min;					//最小值限制
		int32_t Max;					//最大值限制
	
		int32_t out;					//PID输出
	
		
};

2、PID系统初始化

struct Pid_Control AbsoltePid;		//位置式PID
struct Pid_Control IncrementalPid;		//增量式PID


//PID参数初始化
void PID_Init(void)
{
		//位置式PID
		AbsoltePid.Error = 0;
		AbsoltePid.PreError = 0;
		AbsoltePid.LastError = 0;
		AbsoltePid.KP = 8192;
		AbsoltePid.KI = 512;
		AbsoltePid.KD = 1024;
		AbsoltePid.I_AllErr = 0;
		AbsoltePid.P_Parten = 0;
		AbsoltePid.I_Parten = 0;
		AbsoltePid.D_Parten = 0;
		AbsoltePid.Max = 65535;
		AbsoltePid.Min = -65536;
		AbsoltePid.out = 0;
	
		//增量式PID
		IncrementalPid.Error = 0;
		IncrementalPid.PreError = 0;
		IncrementalPid.LastError = 0;
		IncrementalPid.KP = 8192;
		IncrementalPid.KI = 512;
		IncrementalPid.KD = 1024;
		IncrementalPid.I_AllErr = 0;
		IncrementalPid.P_Parten = 0;
		IncrementalPid.I_Parten = 0;
		IncrementalPid.D_Parten = 0;
		IncrementalPid.Max = 65535;
		IncrementalPid.Min = -65536;
		IncrementalPid.out = 0;
}	

3、位置式PID实现

//位置式PID，这里我是用32767表示1，所以才要移位
void Absolute_PID_Calculate(struct Pid_Control* ptr)
{
		ptr->I_AllErr += ptr->Error;
		//计算比例项部分内容
		ptr->P_Parten = ((int32_t)ptr->KP * ptr->Error) >> 15;
		//计算积分项部分内容
		ptr->I_Parten = ((int32_t)ptr->KI * ptr->I_AllErr) >> 15;
		//进行积分抗饱和处理
		if(ptr->I_Parten > ptr->Max - ptr->P_Parten)
		{
				ptr->I_Parten = ptr->Max - ptr->P_Parten;
		}
		else if(ptr->I_Parten < ptr->Min - ptr->P_Parten)
		{
				ptr->I_Parten = ptr->Min - ptr->P_Parten;
		}
		//计算微分项
		ptr->D_Parten = ((int32_t)ptr->KD * (ptr->Error - ptr->PreError)) >> 15;
		//计算输出
		ptr->out = ptr->P_Parten + ptr->I_Parten + ptr->D_Parten;
		//输出限幅
		if(ptr->out >= ptr->Max)
		{
				ptr->out = ptr->Max;
		}
		else if(ptr->out <= ptr->Min)
		{
				ptr->out = ptr->Min;
		}
		//记录上次错误
		ptr->LastError = ptr->PreError;
		ptr->PreError = ptr->Error;
}	

4、增量式PID实现

//增量式PID，这里我是用32767表示1，所以才要移位，实际测试误差过小时无法收敛，
所以在正式使用时需要将误差放大或者是用浮点数
void Incremental_PID_Calculate(struct Pid_Control* ptr)
{
		//计算比例项部分内容
		ptr->P_Parten = ((int32_t)ptr->KP * (ptr->Error - ptr->PreError)) >> 15;
		//计算积分部分
		ptr->I_Parten = ((int32_t)ptr->KI * ptr->Error) >> 15;
		//进行积分抗饱和处理
		if(ptr->I_Parten > ptr->Max - ptr->P_Parten)
		{
				ptr->I_Parten = ptr->Max - ptr->P_Parten;
		}
		else if(ptr->I_Parten < ptr->Min - ptr->P_Parten)
		{
				ptr->I_Parten = ptr->Min - ptr->P_Parten;
		}
		//微分部分计算
		ptr->D_Parten = ((int32_t)ptr->KD * (ptr->Error - 2 * ptr->PreError + ptr->LastError)) >> 15;
		//计算输出
		ptr->out = ptr->P_Parten + ptr->I_Parten + ptr->D_Parten;
		//输出限幅
		if(ptr->out >= ptr->Max)
		{
				ptr->out = ptr->Max;
		}
		else if(ptr->out <= ptr->Min)
		{
				ptr->out = ptr->Min;
		}
		//记录上次错误
		ptr->LastError = ptr->PreError;
		ptr->PreError = ptr->Error;
}

5、测试代码，周期性执行

//位置式PID
AbsoltePid.Error = Target - Curr;
Absolute_PID_Calculate(&AbsoltePid);
Curr = AbsoltePid.out;

//增量式PID
IncrementalPid.Error = Target - Curr;
Incremental_PID_Calculate(&IncrementalPid);
Curr += IncrementalPid.out;