41 lines
1.3 KiB
C
41 lines
1.3 KiB
C
#include "app_pid.h"
|
|
#include <stdio.h>
|
|
|
|
typedef struct PIDController
|
|
{
|
|
float target; // 目标值
|
|
float output; // 控制输出
|
|
float error; // 当前误差
|
|
float prev_error; // 上一次误差
|
|
float integral_error; // 积分误差
|
|
float proportional_gain; // 比例系数
|
|
float integral_gain; // 积分系数
|
|
float derivative_gain; // 微分系数
|
|
} PIDController;
|
|
PIDController pid;
|
|
|
|
void pid_controller_init(float kp, float ki, float kd)
|
|
{
|
|
pid.target = 0.0;
|
|
pid.output = 0.0;
|
|
pid.error = 0.0;
|
|
pid.prev_error = 0.0;
|
|
pid.integral_error = 0.0;
|
|
pid.proportional_gain = kp;
|
|
pid.integral_gain = ki;
|
|
pid.derivative_gain = kd;
|
|
}
|
|
|
|
float pid_position_control(float setpoint, float process_variable)
|
|
{
|
|
pid.target = setpoint;
|
|
pid.error = pid.target - process_variable; // 计算偏差
|
|
|
|
pid.output = pid.proportional_gain * pid.error +
|
|
pid.integral_gain * pid.integral_error +
|
|
pid.derivative_gain * (pid.error - pid.prev_error); // 位置式PID控制器
|
|
pid.prev_error = pid.error; // 保存上一次偏差
|
|
pid.integral_error += pid.error; // 求出偏差的积分
|
|
return pid.output;
|
|
}
|