#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
