位置式、增量式PID算法C语言实现
位置式、增量式PID算法C语言实现
芯片：STM32F107VC
编译器：KEIL4
作者：SY
日期：2017-9-21 15:29:19
概述
PID 算法是一种工控领域常见的控制算法，用于闭环反馈控制。有以下两种分类：
增量式
每次周期性计算出的
PID为增量值，是在上一次控制量的基础上进行的调整。位置式
每次周期性计算出的
PID为绝对的数值，是执行机构实际的位置。
我们使用高级语言的思想去实现两种 PID ，做到对于用户来说，调用相同的接口，内部实现不同的 PID 算法。
代码
pid.h
enum PID_MODE {
PID_INC = 0, //增量式
PID_POS, //位置式
};
struct PID {
enum PID_MODE mode;
float kp; //比例系数
float ki; //积分系数
float kd; //微分系数
double targetPoint; //目标点
double lastError; //Error[-1]
double prevError; //Error[-2]
void (*init)(struct PID *this, double targetPoint); //PID初始化
double (*outputLimit)(struct PID *this, double output); //PID输出限制
void (*setParameter)(struct PID *this, \
float kp, float ki, float kd); //设置PID参数
double (*calculate)(struct PID *this, double samplePoint); //计算PID
};
/* 增量式PID */
struct PID_INC {
struct PID pid;
};
/* 位置式PID */
struct PID_POS {
struct PID pid;
double iSum; //积分和
};其中 struct PID 就是我们提供给用户的接口，可以理解为 抽象类 ，增量式和位置式 PID 都去继承该抽象类，然后实现其中的抽象方法。
对于位置式 PID 拥有自己的成员 iSum 。
pid.c
/*
*********************************************************************************************************
* PID
*********************************************************************************************************
*/
/*
*********************************************************************************************************
* Function Name : PID_Init
* Description : PID初始化
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static void PID_Init(struct PID *this, double targetPoint)
{
this->targetPoint = targetPoint;
this->lastError = 0;
this->prevError = 0;
}
/*
*********************************************************************************************************
* Function Name : PID_OutputLimit
* Description : PID输出限制
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static double PID_OutputLimit(struct PID *this, double output)
{
if (output < 0) {
output = 0;
} else if (output > DIGITAL_THROTTLE_VALVE_MAX_DEGREE) {
output = DIGITAL_THROTTLE_VALVE_MAX_DEGREE;
}
return output;
}
/*
*********************************************************************************************************
* Function Name : PID_SetParameter
* Description : PID设置参数
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static void PID_SetParameter(struct PID *this, float kp, float ki, float kd)
{
this->kp = kp;
this->ki = ki;
this->kd = kd;
}
/*
*********************************************************************************************************
* Function Name : PID_SetTargetValue
* Description : PID设置目标值
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
void PID_SetTargetValue(struct PID *this, double targetPoint)
{
this->targetPoint = targetPoint;
}
/*
*********************************************************************************************************
* Function Name : PID_GetTargetValue
* Description : PID获取目标值
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
double PID_GetTargetValue(struct PID *this)
{
return this->targetPoint;
}
/*
*********************************************************************************************************
* 增量式PID
*********************************************************************************************************
*/
static double PID_IncCalculate(struct PID *this, double samplePoint);
struct PID_INC g_PID_Inc = {
.pid = {
.mode = PID_INC,
.init = PID_Init,
.outputLimit = PID_OutputLimit,
.setParameter = PID_SetParameter,
.calculate = PID_IncCalculate,
},
};
/*
*********************************************************************************************************
* Function Name : PID_IncCalculate
* Description : 增量式PID计算
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static double PID_IncCalculate(struct PID *this, double samplePoint)
{
double nowError = this->targetPoint - samplePoint;
double out = this->kp * nowError +\
this->ki * this->lastError +\
this->kd * this->prevError;
this->prevError = this->lastError;
this->lastError = nowError;
if (this->outputLimit) {
out = this->outputLimit(this, out);
}
return out;
}
/*
*********************************************************************************************************
* 位置式PID
*********************************************************************************************************
*/
static double PID_PosCalculate(struct PID *this, double samplePoint);
static void PID_PosInit(struct PID *this, double targetPoint);
struct PID_POS g_PID_Pos = {
.pid = {
.mode = PID_POS,
.init = PID_PosInit,
.outputLimit = PID_OutputLimit,
.setParameter = PID_SetParameter,
.calculate = PID_PosCalculate,
},
};
/*
*********************************************************************************************************
* Function Name : PID_PosInit
* Description : 位置式PID初始化
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static void PID_PosInit(struct PID *this, double targetPoint)
{
PID_Init(this, targetPoint);
struct PID_POS *pid_Handle = (struct PID_POS *)this;
pid_Handle->iSum = 0;
}
/*
*********************************************************************************************************
* Function Name : PID_PosCalculate
* Description : 位置式PID计算
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
static double PID_PosCalculate(struct PID *this, double samplePoint)
{
struct PID_POS *pid_Handle = (struct PID_POS *)this;
double nowError = this->targetPoint - samplePoint;
this->lastError = nowError;
//积分累计误差
pid_Handle->iSum += nowError;
double out = this->kp * nowError +\
this->ki * pid_Handle->iSum +\
this->kd * (nowError - this->prevError);
this->prevError = nowError;
if (this->outputLimit) {
out = this->outputLimit(this, out);
}
return out;
}对于上述内容：最关键的是两个变量 struct PID_INC g_PID_Inc 和 struct PID_POS g_PID_Pos ，他们针对抽象类实现了各自的算法。
其中有一个很重要的小技巧，举例：
static double PID_PosCalculate(struct PID *this, double samplePoint)
{
struct PID_POS *pid_Handle = (struct PID_POS *)this;
}该函数的接口是 struct PID *this ，但是我们内部将他强制转换为 struct PID_POS *pid_Handle ，这是两种不同的数据类型，为什么可以进行转换呢？而且转换后的数据是正确的？
因为对于 C 语言来说，结构体内部的第一个成员的地址和该结构体变量的地址是一样的，所以可以相互转换，实现向下转型，这就是 C 语言的强大之处。
测试
app.c
struct KernelCtrl {
struct DTV dtv;
struct PID *dtvPid;
};
static struct KernelCtrl g_kernelCtrl;
/*
*********************************************************************************************************
* Function Name : Kernel_Ctrl_Init
* Description : 内核控制初始化
* Input : None
* Output : None
* Return : None
*********************************************************************************************************
*/
void Kernel_Ctrl_Init(void)
{
#if 1
/* 增量式 */
g_kernelCtrl.dtvPid = &g_PID_Inc.pid;
#else
/* 位置式 */
g_kernelCtrl.dtvPid = &g_PID_Pos.pid;
#endif
}只要在初始化时，指定使用哪一种 PID 模式，在调用时两种方式可以使用同一个接口，这样对于用户来说就屏蔽了内部细节。
参考
Pid控制算法-变积分的pid算法的C++实现