PID 算法代码实现

1、控制量相关的结构体
　　有了 PID 的离散化公式之后（https://www.cnblogs.com/The-explosion/p/18838430），我们实现 PID 算法的代码是非常简单的，这里先回顾一下两个 PID 公式所涉及到的所有控制量：

 　　在代码中，我们可以通过一个结构体来管理这些控制量，具体定义如下：

typedef struct PID_ParamTypeDef
{
    __IO float        set_point;         //设定目标
    __IO float        actual_value;      //位置式PID时为实际输出值，增量式PID时为增量值
    __IO float        error_sum;         //误差累计,位置式PID用
    __IO float        proportion;        //比例常数 Proportional Const
    __IO float        integral;          //积分常数 Integral Const
    __IO float        derivative;        //微分常数 Derivative Const
    __IO float        error;             //Error[1],第 k 次偏差 
    __IO float        last_error;        //Error[-1],第 k-1 次偏差
    __IO float        prev_error;        //Error[-2],第 k-2 次偏差，增量式PID用
}PID_ParamTypeDef;

2、PID 算法代码
　　PID 算法的代码实现本质上就是对于两个离散公式的应用，我们先看位置式 PID 代码：

//位置式PID
typedef struct PID_PositionParamTypeDef
{
    __IO float set_point;        //设定目标
    __IO float actual_value;     //实际输出值
    __IO float error_sum;        //误差累计
    __IO float proportion;        //比例常数 Proportional Const
    __IO float integral;        //积分常数 Integral Const
    __IO float derivative;        //微分常数 Derivative Const
    __IO float error;             //Error[1],第 k 次偏差 
    __IO float last_error;        //Error[-1],第 k-1 次偏差
}PID_PositionParamTypeDef;


/************************************************************************************************
  * @ 函 数 名：PID_PositionCalculate
  * @ 功能说明：位置式PID计算
  * @ 参    数：p_PID_PositionParamStruct：pid参数结构体    Feedback_Value：反馈值
  * @ 返 回 值：实际输出值
************************************************************************************************/
float PID_PositionCalculate(PID_PositionParamTypeDef *p_PID_PositionParamStruct, float Feedback_Value)
{
    p_PID_PositionParamStruct->error = p_PID_PositionParamStruct->set_point - Feedback_Value; //计算偏差

    p_PID_PositionParamStruct->error_sum += p_PID_PositionParamStruct->error;

    p_PID_PositionParamStruct->actual_value = (p_PID_PositionParamStruct->proportion * p_PID_PositionParamStruct->error)
                                                +(p_PID_PositionParamStruct->integral * p_PID_PositionParamStruct->error_sum)
                                                +(p_PID_PositionParamStruct->derivative * (p_PID_PositionParamStruct->error - p_PID_PositionParamStruct->last_error));

    p_PID_PositionParamStruct->last_error = p_PID_PositionParamStruct->error;//存储误差，用于下次计算
    
    
    return p_PID_PositionParamStruct->actual_value;  //返回实际输出值
}

　　PID_PositionCalculate函数用来进行位置式 PID 的控制， 该函数的 1个形参： Feedback_value 传入当前系统的实际值，用于计算偏差。 在函数中，我们先计算本次偏差 error，然后把偏差累计，存入 error_sum 成员当中，接着根据位置式的公式进行三个环节的计算，计算后的期望输出存入 actual_value 成员当中，然后存储本次偏差，最后返回期望输出值。

　　接着我们看增量式 PID 代码：

//增量式PID
typedef struct PID_IncrementParamTypeDef
{
    __IO float set_point;        //设定目标
    __IO float actual_value;     //增量值
    __IO float proportion;        //比例常数 Proportional Const
    __IO float integral;        //积分常数 Integral Const
    __IO float derivative;        //微分常数 Derivative Const
    __IO float error;             //Error[1],第 k 次偏差 
    __IO float last_error;        //Error[-1],第 k-1 次偏差
    __IO float prev_error;        //Error[-2],第 k-2 次偏差
}PID_IncrementParamTypeDef;




#if 0

/************************************************************************************************
  * @ 函 数 名：PID_IncrementCalculate
  * @ 功能说明：增量式PID计算
  * @ 参    数：p_PID_IncrementParamStruct：pid参数结构体    Feedback_Value：反馈值
  * @ 返 回 值：增量值
************************************************************************************************/
float PID_IncrementCalculate(PID_IncrementParamTypdeDef *p_PID_IncrementParamStruct, float Feedback_Value)
{
    p_PID_IncrementParamStruct->error = p_PID_IncrementParamStruct->set_point - Feedback_Value; //计算偏差


    p_PID_IncrementParamStruct->actual_value = (p_PID_IncrementParamStruct->proportion * p_PID_IncrementParamStruct->error)
                                                -(p_PID_IncrementParamStruct->integral * p_PID_IncrementParamStruct->last_error)
                                                +(p_PID_IncrementParamStruct->derivative * p_PID_IncrementParamStruct->prev_error);

    p_PID_IncrementParamStruct->prev_error = p_PID_IncrementParamStruct->last_error;//存储误差，用于下次计算
    p_PID_IncrementParamStruct->last_error = p_PID_IncrementParamStruct->error;
    
    
    return p_PID_IncrementParamStruct->actual_value;  //返回增量值
}

#else

/************************************************************************************************
  * @ 函 数 名：PID_IncrementCalculate
  * @ 功能说明：增量式PID计算
  * @ 参    数：p_PID_IncrementParamStruct：pid参数结构体  Feedback_Value：反馈值
  * @ 返 回 值：增量值
************************************************************************************************/
float PID_IncrementCalculate(PID_IncrementParamTypeDef *p_PID_IncrementParamStruct, float Feedback_Value)
{
    p_PID_IncrementParamStruct->error = p_PID_IncrementParamStruct->set_point - Feedback_Value; //计算偏差


    p_PID_IncrementParamStruct->actual_value = (p_PID_IncrementParamStruct->proportion * (p_PID_IncrementParamStruct->error - p_PID_IncrementParamStruct->last_error))
                                                -(p_PID_IncrementParamStruct->integral * p_PID_IncrementParamStruct->error)
                                                +(p_PID_IncrementParamStruct->derivative * (p_PID_IncrementParamStruct->error - 2.0f*p_PID_IncrementParamStruct->last_error + p_PID_IncrementParamStruct->prev_error));

    p_PID_IncrementParamStruct->prev_error = p_PID_IncrementParamStruct->last_error;//存储误差，用于下次计算
    p_PID_IncrementParamStruct->last_error = p_PID_IncrementParamStruct->error;
    
    
    return p_PID_IncrementParamStruct->actual_value;                  //返回增量值
}


#endif

　　增量式 PID 的代码实现和位置式是非常类似的，所以我们在实际的代码实现中，可以通过一个宏定义来切换这两种不同的算法，值得注意的是，增量式 PID 输出的是调节量，所以计算期望输出值 actual_value 的时候是自增运算，这一点和位置式 PID 是不一样的。