PID控制算法（增量式、位置式）

#include "pid_control.h" //定义PID参数结构体 Pid_para pid; //判断是 位置式PID 还是 增量式PID #if PID_SELECT /*-------------------------------------------------------------- - Function name : PID_init - Description : PID参数初始化 - Parameter : - - Return : - ---------------------------------------------------------------*/ void PID_init() { pid.ExpectSpeed = 0.0f; pid.ActualSpeed = 0.0f; pid.err = 0.0f; pid.err_last = 0.0f; pid.Kp = 0.0f; pid.Ki = 0.0f; pid.Kd = 0.0f; pid.integral = 0.0f; } /*-------------------------------------------------------------- - Function name : PID_realize - Description : PID实现（位置式） - Parameter : expect_speed：期望值 actual_speed：实际值 - Return : 返回计算结果 ---------------------------------------------------------------*/ float PID_realize(float expect_speed,float actual_speed) { float P,I,D,speed_out; pid.ExpectSpeed = expect_speed; //期望值 pid.ActualSpeed = actual_speed; //实际值 pid.err = pid.ExpectSpeed - pid.ActualSpeed; //当前偏差 P = pid.Kp*pid.err; //比例P计算公式 I = pid.Ki*pid.integral; //积分I计算公式 D = pid.Kd*(pid.err-pid.err_last); //微分D计算公式 pid.integral += pid.err; //积分值，偏差累加，更新积分值 pid.err_last=pid.err; //偏差更新 speed_out = P + I + D; //计算值 return speed_out; } #else /*-------------------------------------------------------------- - Function name : PID_init - Description : PID参数初始化 - Parameter : - - Return : - ---------------------------------------------------------------*/ void PID_init() { pid.ExpectSpeed = 0.0f; pid.ActualSpeed = 0.0f; pid.err = 0.0f; pid.err_last = 0.0f; pid.err_prev = 0.0f; pid.Kp = 0.0f; pid.Ki = 0.0f; pid.Kd = 0.0f; } /*-------------------------------------------------------------- - Function name : PID_realize - Description : PID实现（增量式） - Parameter : expect_speed：期望值 actual_speed：实际值 - Return : 返回计算结果 ---------------------------------------------------------------*/ float PID_realize(float expect_speed,float actual_speed) { float P, I, D,speed_add; pid.ExpectSpeed = expect_speed; //期望值 pid.ActualSpeed = actual_speed; //实际值 pid.err = pid.ExpectSpeed - pid.ActualSpeed; //偏差值 P = pid.Kp*(pid.err - pid.err_last); //比例P输出公式 I = pid.Ki*pid.err; //积分I输出公式 D = pid.Kd*(pid.err - 2 * pid.err_last + pid.err_prev); //微分D输出公式 pid.err_prev = pid.err_last; //偏差更新 pid.err_last = pid.err; speed_add = P + I + D; //增量值 pid.ActualSpeed += speed_add; //上一次记忆值加上增量 return pid.ActualSpeed; } #endif