位置式、增量式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
模式,在调用时两种方式可以使用同一个接口,这样对于用户来说就屏蔽了内部细节。
参考
Pid控制算法-变积分的pid算法的C++实现
再牛逼的梦想也架不住傻逼似的坚持