【PID】位置式、增量式PID算法C语言实现

位置式、增量式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++实现

 

 

【来源】https://blog.csdn.net/sinat_20006769/article/details/79676714

 

posted @ 2018-09-25 17:33  壹点灵异  阅读(3776)  评论(0)  编辑  收藏  举报