PID算法的C语言实现

float PIDcal(float setpoint,float actual_position)
{
	static float pre_error = 0;         // 定义前一次误差
	static float integral = 0;          // 定义积分项累加值
	float error;                        // 定义最新误差
	float derivative;                   // 定义微分项
	float output;                       // 定义输出

	// 计算误差
	error = setpoint - actual_position;

	// 死区设置，在误差极小的情况下不做积分处理，保证稳定性
	if(my_abs(error) > epsilon)
	{
	   integral = integral + error*dt;   // 积分项,做完壁障之后，积分清零
	}

	derivative = (error - pre_error)/dt; // 微分项

	output = Kp*error + Ki*integral + Kd*derivative; // 各项乘以系数得到PID输出

	// 输出限幅，确保输出不会太夸张
	if(output > MAX)
	{
		output = MAX;
	}
	else if(output < MIN)
	{
		output = MIN;
	}

        // 更新误差
        pre_error = error;

        return output;
}