/* =================================================================================
File name: PID_GRANDO.H
===================================================================================*/#ifndef __PID_H__
#define __PID_H__typedef struct { _iq Ref; // Input: reference set-point 参考点_iq Fbk; // Input: feedback 反馈_iq Out; // Output: controller output 控制器输出 _iq c1; // Internal: derivative filter coefficient 1 导数滤波系数1_iq c2; // Internal: derivative filter coefficient 2 导数滤波系数2} PID_TERMINALS;// note: c1 & c2 placed here to keep structure size under 8 wordstypedef struct { _iq Kr; // Parameter: reference set-point weighting 参考设定点权重_iq Kp; // Parameter: proportional loop gain 比例回路增益_iq Ki; // Parameter: integral gain 积分增益_iq Kd; // Parameter: derivative gain 导数增益_iq Km; // Parameter: derivative weighting 导数加权_iq Umax; // Parameter: upper saturation limit 饱和上限_iq Umin; // Parameter: lower saturation limit 饱和下限} PID_PARAMETERS;typedef struct { _iq up; // Data: proportional term 比例项_iq ui; // Data: integral term 积分项_iq ud; // Data: derivative term 导数项_iq v1; // Data: pre-saturated controller output 预饱和控制器输出_iq i1; // Data: integrator storage: ui(k-1) 积分器存储:ui(k-1)_iq d1; // Data: differentiator storage: ud(k-1) 积分器存储:ui(k-1)_iq d2; // Data: differentiator storage: d2(k-1) 微分器存储:d2(k-1)_iq w1; // Data: saturation record: [u(k-1) - v(k-1)] 微分器存储:d2(k-1)} PID_DATA;typedef struct { PID_TERMINALS term;PID_PARAMETERS param;PID_DATA data;} PID_CONTROLLER;/*-----------------------------------------------------------------------------
Default initalisation values for the PID objects
-----------------------------------------------------------------------------*/ #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 Macro Definition
------------------------------------------------------------------------------*/#define PID_MACRO(v) \\/* proportional term */ \v.data.up = _IQmpy(v.param.Kr, v.term.Ref) - v.term.Fbk; \\/* integral term */ \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; \\/* derivative term */ \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); \\/* control output */ \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__
Example
The following pseudo code provides the information about the module usage. /* Instance the PID module */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; // Pass _iq inputs to pid1pid1.Fbk = input1_2; // Pass _iq inputs to pid1 PID_MACRO(pid1); // Call compute macro for pid1 output1 = pid1.Out; // Access the output of pid1
}
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！
