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;
 
}