TI公司的增量式PID算法

/* =================================================================================
文件名：PID_GRANDO.H 
===================================================================================*/


#ifndef __PID_H__
#define __PID_H__

typedef struct {  _iq  Ref;      // 输入：参考设定值
      _iq  Fbk;      // 输入：反馈值
      _iq  Out;      // 输出：控制器输出
      _iq  c1;       // 内部：微分滤波器系数1
      _iq  c2;       // 内部：微分滤波器系数2
    } PID_TERMINALS;
    // 注意：将c1和c2放在这里可以保持结构体大小为8个字

typedef struct {  _iq  Kr;     // 参数：参考设定值权重
      _iq  Kp;     // 参数：比例增益
      _iq  Ki;     // 参数：积分增益
      _iq  Kd;     // 参数：微分增益
      _iq  Km;     // 参数：微分权重
      _iq  Umax;   // 参数：上饱和限制
      _iq  Umin;   // 参数：下饱和限制
    } PID_PARAMETERS;

typedef struct {  _iq  up;     // 数据：比例项
      _iq  ui;     // 数据：积分项
      _iq  ud;     // 数据：微分项
      _iq  v1;     // 数据：饱和前的控制器输出
      _iq  i1;     // 数据：积分器存储器：ui(k-1)
      _iq  d1;     // 数据：微分器存储器：ud(k-1)
      _iq  d2;     // 数据：微分器存储器：d2(k-1)
      _iq  w1;     // 数据：饱和记录：[u(k-1) - v(k-1)]
    } PID_DATA;


typedef struct {  PID_TERMINALS term;
      PID_PARAMETERS param;
      PID_DATA  data;
    } PID_CONTROLLER;

/*-----------------------------------------------------------------------------
PID对象的默认初始化值
-----------------------------------------------------------------------------*/

#define PID_TERM_DEFAULTS {    \
                           0,  \
                           0,  \
                           0,  \
                           0,  \
                           0   \
                   }

#define PID_PARAM_DEFAULTS {         \
                           _IQ(1.0), \
                           _IQ(1.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(1.0), \
                           _IQ(1.0), \
                           _IQ(-1.0) \
                   }

#define PID_DATA_DEFAULTS {          \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(0.0), \
                           _IQ(1.0)  \
                   }


/*------------------------------------------------------------------------------
PID宏定义
------------------------------------------------------------------------------*/

#define PID_MACRO(v)                                              \
                                                                  \
/* 比例项 */                                                      \
v.data.up = _IQmpy(v.param.Kr, v.term.Ref) - v.term.Fbk;          \
                                                                  \
/* 积分项 */                                                      \
v.data.ui = _IQmpy(v.param.Ki, _IQmpy(v.data.w1,                  \
(v.term.Ref - v.term.Fbk))) + v.data.i1;                          \
v.data.i1 = v.data.ui;                                            \
                                                                  \
/* 微分项 */                                                      \
v.data.d2 = _IQmpy(v.param.Kd, _IQmpy(v.term.c1,                  \
(_IQmpy(v.term.Ref, v.param.Km) - v.term.Fbk))) - v.data.d2;      \
v.data.ud = v.data.d2 + v.data.d1;                                \
v.data.d1 = _IQmpy(v.data.ud, v.term.c2);                         \
                                                                  \
/* 控制器输出 */                                                  \
v.data.v1 = _IQmpy(v.param.Kp,                                    \
(v.data.up + v.data.ui + v.data.ud));                             \
v.term.Out= _IQsat(v.data.v1, v.param.Umax, v.param.Umin);        \
v.data.w1 = (v.term.Out == v.data.v1) ? _IQ(1.0) : _IQ(0.0);      \
 
#endif // __PID_H__

这段代码是一个基本的PID控制器的实现。PID控制器是一种常用的控制算法，用于根据系统的反馈值和期望值来生成输出控制信号。在该代码中，PID控制器被定义为一个结构体，包含了输入、输出、参数和数据等成员。

该代码中的结构体PID_TERMINALS定义了PID控制器的输入和输出，包括参考值Ref、反馈值Fbk和控制器输出Out等。

结构体PID_PARAMETERS定义了PID控制器的参数，包括参考值的权重Kr、比例增益Kp、积分增益Ki、微分增益Kd、微分权重Km以及饱和上下限Umax和Umin等。

结构体PID_DATA定义了PID控制器的数据，包括比例项up、积分项ui、微分项ud、饱和前的控制器输出v1、积分器存储i1、微分器存储d1、微分器存储d2和饱和记录w1等。

最后，结构体PID_CONTROLLER将上述三个结构体进行组合，表示完整的PID控制器。

宏定义部分定义了PID控制器的默认初始值，包括PID_TERMINALS的默认值、PID_PARAMETERS的默认值和PID_DATA的默认值。

PID_MACRO宏定义了PID控制器的计算过程，根据PID控制器的输入、参数和数据，计算出比例、积分和微分项的值，并根据控制输出的饱和上下限进行饱和处理，最后更新饱和记录。

/* 实例化PID模块 */
PID pid1 = {PID_TERM_DEFAULTS, PID_PARAM_DEFAULTS, PID_DATA_DEFAULTS};

main() {
    pid1.param.Kp = _IQ(0.5);
    pid1.param.Ki = _IQ(0.005);
    pid1.param.Kd = _IQ(0);
    pid1.param.Kr = _IQ(1.0);
    pid1.param.Km = _IQ(1.0);
    pid1.param.Umax = _IQ(1.0);
    pid1.param.Umin = _IQ(-1.0);
}

void interrupt periodic_interrupt_isr() {
    pid1.Ref = input1_1;   // 将输入值传递给pid1的Ref
    pid1.Fbk = input1_2;   // 将输入值传递给pid1的Fbk
    PID_MACRO(pid1); // 调用计算宏计算pid1的输出
    output1 = pid1.Out; // 获取pid1的输出值
}

这段代码实例化了一个PID模块，并定义了名为pid1的PID_CONTROLLER结构体变量。

在main函数中，设置了pid1的参数值，包括比例增益Kp、积分增益Ki、微分增益Kd、参考设定值权重Kr、微分权重Km以及饱和上下限。

在周期中断的中断服务函数中，将输入值input1_1和input1_2赋值给pid1的Ref和Fbk成员变量。然后调用PID_MACRO宏来计算pid1的控制输出值。最后，将pid1的输出值赋值给output1。