【回転】位置式、インクリメンタルPIDアルゴリズムC言語実装

34103 ワード

位置式、インクリメンタルPIDアルゴリズムC言語実現
チップ:STM 32 F 107 VC
コンパイラ:KEIL 4
作者:SY
日付:2017-9-21 15:29:19
概要PIDアルゴリズムは、閉ループフィードバック制御のための工業制御分野でよく見られる制御アルゴリズムである.次の2つの分類があります.

増分式で周期的に算出されるPIDは増分値であり、前回の制御量に基づいた調整である.

位置式で周期的に算出されるPIDは絶対値であり、実行機構の実際の位置である.

我々は高度な言語の思想を用いて2つの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 }

上記の内容について、最も重要なのは、抽象クラスに対してそれぞれのアルゴリズムを実現する2つの変数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に強制的に変換されています.これは2つの異なるデータ型ですが、なぜ変換できるのでしょうか.そして変換されたデータは正しいですか?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://www.cnblogs.com/skullboyer/p/9700762.html

C言語はrawフォーマット画像の読み込みとアクセスを実現する

k-medoidsクラスタリングアルゴリズム