STM32的PID例程

这是我自己写的关于pid的一个位置式算法的部分，在处理程序中用了比较简单的单纯的返回一个值，可以根据自己的需要改写一下。。。

定义PID结构体，包含了计算中用到的各种变量
=====================================================================================================*/
typedef struct PID
{
        double SetPoint;   // 设定目标值r(t) Desired Value

        double Proportion; // 比例常数k(p) Proportional Const
        double Integral;   // 积分常数k(i) Integral Const
        double Derivative; // 微分常数k(d) Derivative Const

        double LastError; // 微分部分的 最新偏差e(k)    Error[-1]
        double PrevError; // 微分部分的 最新偏差e(k-1)  Error[-2]

        double SumError;  // 积分部分的 累加偏差   Sums of Errors
}PID;

/*====================================================================================================
　　 Initialize PID Structure----对定义的PID结构体类型的pp所占用的地址空间都给清零
=====================================================================================================*/
void PIDInit(PID *pp)
{
        memset ( pp,0,sizeof(PID));
}
/*====================================================================================================
　　 PID计算部分----位置式PID算法
=====================================================================================================*/
double PIDCalc( PID *pp, double NextPoint )
{
        double dError, Error;
        Error = pp->SetPoint - NextPoint;       // 比例部分的  最新一次采样计算的偏差
        pp->SumError += Error;                  // 积分部分的  所有偏差的累加
        dError = pp->LastError - pp->PrevError; // 微分部分的  最新一次偏差和前一次偏差的减法计算
        pp->PrevError = pp->LastError;          // 微分部分的  更新上一次偏差为最新偏差
        pp->LastError = Error;                  // 微分部分的  更新最新偏差为当前偏差
        return (pp->Proportion * Error  +  pp->Integral * pp->SumError  +  pp->Derivative * dError);
                                                // 比例项部分 + 积分项部分 + 微分项部分
}
/*====================================================================================================
　　 PID计算部分----增量式PID算法
=====================================================================================================*/
double PIDCalc_delt( PID *pp, double NextPoint )
{
        double dError, Error;
        Error = pp->SetPoint - NextPoint;       // 比例部分的  最新一次采样计算的偏差
        dError= pp->Proportion * Error  +  pp->Integral * pp->LastError  +  pp->Derivative * pp->PrevError;
        pp->PrevError = pp->LastError;          // 微分部分的  更新上一次偏差为最新偏差
        pp->LastError = Error;
        return (dError);
}
/*====================================================================================================
　　 PID计算部分----PID处理部分
=====================================================================================================*/
uint32_t actuator(double rDelta) // Dummy Actuator Function 根据计算结果u进行处理
{
        return 0;        
}