#include <stdio.h>
#include <stdlib.h>
typedef struct {
float Kp; // 比例增益
float Ki; // 积分增益
float Kd; // 微分增益
float error; // 当前误差
float prev_error; // 上一次误差
float integral; // 积分项
} PID;
void init_PID(PID *pid, float kp, float ki, float kd) {
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
pid->error = 0;
pid->prev_error = 0;
pid->integral = 0;
}
float update_PID_position(PID *pid, float setpoint, float feedback) {
float error = setpoint - feedback;
pid->integral += pid->Ki * error;
float derivative = pid->Kd * (error - pid->prev_error);
pid->prev_error = error;
return pid->Kp * error + pid->integral + derivative;
}
|