PID算法，增量式和位置式控制，实现代码

#include "pid.h"

#include "main.h"



// pid结构体的初始化

void PIDStructInit(_PID *pid,float target,float p,float i,float d)

{

        pid->p = p;

        pid->i = i;

        pid->d = d;

        pid->target = target;

        pid->err_curr = 0;

        pid->err_last = 0;

        pid->err_sum = 0;

}



// pid 位置式计算

float PID_Postion(_PID *pid,float current)

{

        volatile float Pout;   // 计算的输出值

        pid->err_curr = pid->target - current;   // 计算当前误差

        pid->err_sum += pid->err_curr;   //积分

        Pout = (pid->p * pid->err_curr) + (pid->i * pid->err_sum) + (pid->d * (pid->err_curr - pid->err_last));   // 计算pid的值

        printf("P = %.3f   I = %.3f   D = %.3f\r\n",(pid->p * pid->err_curr) , (pid->i * pid->err_sum) , (pid->d * (pid->err_curr - pid->err_last)));

        pid->err_last = pid->err_curr;   // 记录上一次的误差为当前这一次

        return Pout;

}

typedef struct _PID

{

        volatile float p;

        volatile float i;

        volatile float d;

        float err_curr;   // 当前误差

        float err_last;  // 上次误差

        float err_sum;   // 误差和  积分

        float err_last_last;  // 上上次的误差

        float target;    // 目标值

        float Out;  // 增量 输出的值

}_PID;



// pid结构体的初始化

void PIDStructInit(_PID *pid,float target,float p,float i,float d)

{

        pid->p = p;

        pid->i = i;

        pid->d = d;

        pid->target = target;

        pid->err_curr = 0;

        pid->err_last = 0;

        pid->err_last_last = 0;

        pid->err_sum = 0;

        pid->Out  = 0;

}





// 增量式PID 计算

float PID_Incremental(_PID *pid,float current)

{

        volatile float Pout;   // 计算的输出值

        pid->err_curr = pid->target - current;   // 计算当前误差

        Pout = (pid->p * (pid->err_curr - pid->err_last)) + (pid->i * pid->err_last) + (pid->d * ((pid->err_curr - 2* pid->err_last + pid->err_last_last)));   // 计算pid的值

        printf("P = %.3f   I = %.3f   D = %.3f\r\n",(pid->p * (pid->err_curr - pid->err_last)) , (pid->i * pid->err_last) , (pid->d * ((pid->err_curr - 2* pid->err_last + pid->err_last_last))));

        pid->err_last_last = pid->err_last;   // 上上次更新为上次

        pid->err_last = pid->err_curr;     // 将上次的值更新为这次

        pid->Out += Pout;

        return pid->Out;

}