PID算法控制代码

.h

#ifndef PID_H
#define PID_H
 
#include <stdint.h>
 
typedef struct
{
    float kp;          // 比例系数
    float ki;          // 积分系数
    float kd;          // 微分系数
    float SP;          // 目标值
 
    uint64_t last_time; // 上次计算时间
    float last_int;    // 上次积分值
    float last_error;  // 上一次误差值
 
    float upper_limit; // 输出上限
    float lower_limit; // 输出下限
} PID_Controller;
 
void PID_Init(PID_Controller *pid, float kp, float ki, float kd);
void PID_ChangeSP(PID_Controller *pid, float sp);
float PID_Compute(PID_Controller *pid, float FB);
void PID_SetOutputLimits(PID_Controller *pid, float lower, float upper);
 
#endif // !PID_H

.c

#include "pid.h"
#include "delay.h"
 
// @简介: 初始化PID控制器
// @参数: kp  - 比例系数
// @参数: ki  - 积分系数
// @参数: kd  - 微分系数
void PID_Init(PID_Controller *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
 
    pid->SP = 0.0f;
    pid->last_time = 0;
    pid->last_int = 0.0f;
    pid->last_error = 0.0f;
 
    pid->upper_limit = 3.4e+38f; // 默认接近float类型的最大值，即输出无上限
    pid->lower_limit = -3.4e+38f; // 默认接近float类型的最小值，即输出无下限
 
}
 
// @简介: 更改PID目标值
// @参数: sp  - 新的目标值
void PID_ChangeSP(PID_Controller *pid, float sp)
{
    pid->SP = sp;
}
 
// @简介: 计算一次PID输出值
// @参数: FB  - 反馈值
// @返回值: PID计算结果
float PID_Compute(PID_Controller *pid, float FB)
{
    float error = pid->SP - FB;
 
    uint64_t current_time = GetUs(); // 获取当前时间
 
    uint64_t delta_time = current_time - pid->last_time;
 
    float err_int = 0.0f;
    float err_dev = 0.0f;
 
    if( delta_time != 0 ) //第一次计算时跳过积分和微分计算
    {
        err_int = pid->last_int + error * delta_time;
        err_dev = (error - pid->last_error) / delta_time;
    }
 
    float output = pid->kp * error + pid->ki * err_int + pid->kd * err_dev;
    // 输出限幅
    if (output > pid->upper_limit) output = pid->upper_limit;
    if (output < pid->lower_limit) output = pid->lower_limit;
 
    // 更新上一次误差值
    pid->last_error = error;
    pid->last_int = err_int;
    pid->last_time = current_time;
 
    //积分限幅
    if(err_int > pid->upper_limit) pid->last_int = pid->upper_limit;
    if(err_int < pid->lower_limit) pid->last_int = pid->lower_limit;
 
    return output;
}
 
// @简介: 设置PID输出限幅
// @参数: lower  - 输出下限
// @参数: upper  - 输出上限
void PID_SetOutputLimits(PID_Controller *pid, float lower, float upper)
{
    pid->lower_limit = lower;
    pid->upper_limit = upper;
}
 
// @简介: pid重置函数
void PID_Reset(PID_Controller *pid)
{
    pid->last_time = 0;
    pid->last_int = 0.0f;
    pid->last_error = 0.0f;
}