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