位置式、增量式PID算法C语言实现
芯片：STM32F107VC
编译器：KEIL4
作者：SY
日期：2017-9-21 15:29:19
概述
PID 算法是一种工控领域常见的控制算法，用于闭环反馈控制。有以下两种分类：
-
增量式
每次周期性计算出的
PID为增量值，是在上一次控制量的基础上进行的调整。 -
位置式
每次周期性计算出的
PID为绝对的数值，是执行机构实际的位置。
我们使用高级语言的思想去实现两种 PID ，做到对于用户来说，调用相同的接口，内部实现不同的 PID 算法。
代码
pid.h
1 enum PID_MODE { 2 PID_INC = 0, //增量式 3 PID_POS, //位置式 4 }; 5 6 struct PID { 7 enum PID_MODE mode; 8 9 float kp; //比例系数 10 float ki; //积分系数 11 float kd; //微分系数 12 13 double targetPoint; //目标点 14 double lastError; //Error[-1] 15 double prevError; //Error[-2] 16 17 void (*init)(struct PID *this, double targetPoint); //PID初始化 18 double (*outputLimit)(struct PID *this, double output); //PID输出限制 19 void (*setParameter)(struct PID *this, 20 float kp, float ki, float kd); //设置PID参数 21 double (*calculate)(struct PID *this, double samplePoint); //计算PID 22 }; 23 24 /* 增量式PID */ 25 struct PID_INC { 26 struct PID pid; 27 }; 28 29 /* 位置式PID */ 30 struct PID_POS { 31 struct PID pid; 32 double iSum; //积分和 33 };
其中 struct PID 就是我们提供给用户的接口，可以理解为 抽象类 ，增量式和位置式 PID 都去继承该抽象类，然后实现其中的抽象方法。
对于位置式 PID 拥有自己的成员 iSum 。
pid.c
1 /* 2 ********************************************************************************************************* 3 * PID 4 ********************************************************************************************************* 5 */ 6 /* 7 ********************************************************************************************************* 8 * Function Name : PID_Init 9 * Description : PID初始化 10 * Input : None 11 * Output : None 12 * Return : None 13 ********************************************************************************************************* 14 */ 15 static void PID_Init(struct PID *this, double targetPoint) 16 { 17 this->targetPoint = targetPoint; 18 this->lastError = 0; 19 this->prevError = 0; 20 } 21 22 /* 23 ********************************************************************************************************* 24 * Function Name : PID_OutputLimit 25 * Description : PID输出限制 26 * Input : None 27 * Output : None 28 * Return : None 29 ********************************************************************************************************* 30 */ 31 static double PID_OutputLimit(struct PID *this, double output) 32 { 33 if (output < 0) { 34 output = 0; 35 } else if (output > DIGITAL_THROTTLE_VALVE_MAX_DEGREE) { 36 output = DIGITAL_THROTTLE_VALVE_MAX_DEGREE; 37 } 38 return output; 39 } 40 41 /* 42 ********************************************************************************************************* 43 * Function Name : PID_SetParameter 44 * Description : PID设置参数 45 * Input : None 46 * Output : None 47 * Return : None 48 ********************************************************************************************************* 49 */ 50 static void PID_SetParameter(struct PID *this, float kp, float ki, float kd) 51 { 52 this->kp = kp; 53 this->ki = ki; 54 this->kd = kd; 55 } 56 57 /* 58 ********************************************************************************************************* 59 * Function Name : PID_SetTargetValue 60 * Description : PID设置目标值 61 * Input : None 62 * Output : None 63 * Return : None 64 ********************************************************************************************************* 65 */ 66 void PID_SetTargetValue(struct PID *this, double targetPoint) 67 { 68 this->targetPoint = targetPoint; 69 } 70 71 /* 72 ********************************************************************************************************* 73 * Function Name : PID_GetTargetValue 74 * Description : PID获取目标值 75 * Input : None 76 * Output : None 77 * Return : None 78 ********************************************************************************************************* 79 */ 80 double PID_GetTargetValue(struct PID *this) 81 { 82 return this->targetPoint; 83 } 84 85 /* 86 ********************************************************************************************************* 87 * 增量式PID 88 ********************************************************************************************************* 89 */ 90 static double PID_IncCalculate(struct PID *this, double samplePoint); 91 92 struct PID_INC g_PID_Inc = { 93 .pid = { 94 .mode = PID_INC, 95 .init = PID_Init, 96 .outputLimit = PID_OutputLimit, 97 .setParameter = PID_SetParameter, 98 .calculate = PID_IncCalculate, 99 }, 100 }; 101 102 /* 103 ********************************************************************************************************* 104 * Function Name : PID_IncCalculate 105 * Description : 增量式PID计算 106 * Input : None 107 * Output : None 108 * Return : None 109 ********************************************************************************************************* 110 */ 111 static double PID_IncCalculate(struct PID *this, double samplePoint) 112 { 113 double nowError = this->targetPoint - samplePoint; 114 double out = this->kp * nowError + 115 this->ki * this->lastError + 116 this->kd * this->prevError; 117 this->prevError = this->lastError; 118 this->lastError = nowError; 119 120 if (this->outputLimit) { 121 out = this->outputLimit(this, out); 122 } 123 124 return out; 125 } 126 127 /* 128 ********************************************************************************************************* 129 * 位置式PID 130 ********************************************************************************************************* 131 */ 132 static double PID_PosCalculate(struct PID *this, double samplePoint); 133 static void PID_PosInit(struct PID *this, double targetPoint); 134 135 struct PID_POS g_PID_Pos = { 136 .pid = { 137 .mode = PID_POS, 138 .init = PID_PosInit, 139 .outputLimit = PID_OutputLimit, 140 .setParameter = PID_SetParameter, 141 .calculate = PID_PosCalculate, 142 }, 143 }; 144 145 /* 146 ********************************************************************************************************* 147 * Function Name : PID_PosInit 148 * Description : 位置式PID初始化 149 * Input : None 150 * Output : None 151 * Return : None 152 ********************************************************************************************************* 153 */ 154 static void PID_PosInit(struct PID *this, double targetPoint) 155 { 156 PID_Init(this, targetPoint); 157 struct PID_POS *pid_Handle = (struct PID_POS *)this; 158 pid_Handle->iSum = 0; 159 } 160 161 /* 162 ********************************************************************************************************* 163 * Function Name : PID_PosCalculate 164 * Description : 位置式PID计算 165 * Input : None 166 * Output : None 167 * Return : None 168 ********************************************************************************************************* 169 */ 170 static double PID_PosCalculate(struct PID *this, double samplePoint) 171 { 172 struct PID_POS *pid_Handle = (struct PID_POS *)this; 173 174 double nowError = this->targetPoint - samplePoint; 175 this->lastError = nowError; 176 //积分累计误差 177 pid_Handle->iSum += nowError; 178 double out = this->kp * nowError + 179 this->ki * pid_Handle->iSum + 180 this->kd * (nowError - this->prevError); 181 this->prevError = nowError; 182 183 if (this->outputLimit) { 184 out = this->outputLimit(this, out); 185 } 186 187 return out; 188 }
对于上述内容：最关键的是两个变量 struct PID_INC g_PID_Inc 和 struct PID_POS g_PID_Pos ，他们针对抽象类实现了各自的算法。
其中有一个很重要的小技巧，举例：
1 static double PID_PosCalculate(struct PID *this, double samplePoint) 2 { 3 struct PID_POS *pid_Handle = (struct PID_POS *)this; 4 }
该函数的接口是 struct PID *this ，但是我们内部将他强制转换为 struct PID_POS *pid_Handle ，这是两种不同的数据类型，为什么可以进行转换呢？而且转换后的数据是正确的？
因为对于 C 语言来说，结构体内部的第一个成员的地址和该结构体变量的地址是一样的，所以可以相互转换，实现向下转型，这就是 C 语言的强大之处。
测试
app.c
1 struct KernelCtrl { 2 struct DTV dtv; 3 struct PID *dtvPid; 4 }; 5 6 static struct KernelCtrl g_kernelCtrl; 7 8 /* 9 ********************************************************************************************************* 10 * Function Name : Kernel_Ctrl_Init 11 * Description : 内核控制初始化 12 * Input : None 13 * Output : None 14 * Return : None 15 ********************************************************************************************************* 16 */ 17 void Kernel_Ctrl_Init(void) 18 { 19 #if 1 20 /* 增量式 */ 21 g_kernelCtrl.dtvPid = &g_PID_Inc.pid; 22 #else 23 /* 位置式 */ 24 g_kernelCtrl.dtvPid = &g_PID_Pos.pid; 25 #endif 26 }
只要在初始化时，指定使用哪一种 PID 模式，在调用时两种方式可以使用同一个接口，这样对于用户来说就屏蔽了内部细节。
参考
【来源】