// 文件编码: GB18030
/*
 * pid.c
 *
 *  Created on: 2025年3月25日
 *      Author: LHYe200
 */

#include "pid.h"
#include "math.h"


void PID_Init(PID *pid, float kp, float ki, float kd, float integral_max, float output_max, float output_min)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->integral_max = integral_max;

    pid->output_max = output_max;
    pid->output_min = output_min;

    pid->p_output = 0;
    pid->i_output = 0;
    pid->d_output = 0;
    pid->output = 0;

    pid->err = 0;
    pid->err_last = 0;
    pid->last_derivative = 0;
    pid->integral = 0;
}

/*************************************************************************
*  函数名称：float constrain_float(float amt, float low, float high)
*  功能说明：限幅函数
*  参数说明：
* @param    amt   ： 参数
* @param    low   ： 最低值
* @param    high  ： 最高值
*  函数返回：限幅结果
*************************************************************************/
float constrain_float(float amt, float low, float high)
{
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}


void PID_Reset(PID *pid)
{
    pid->p_output = 0;
    pid->i_output = 0;
    pid->d_output = 0;
    pid->output = 0;

    pid->err = 0;
    pid->err_last = 0;
    pid->last_derivative = 0;
    pid->integral = 0;
}

/*************************************************************************
*  函数名称：float PidLocCtrl(pid_param_t * pid, float err)
*  功能说明：pid位置式控制器输出
*  参数说明：
* @param    pid     pid参数
* @param    err   pid输入误差
*  函数返回：PID输出结果
*************************************************************************/

float PID_Loc_Ctrl(PID * pid, float err)
{
    /* 累积误差 */
    pid->err = err;
    pid->integral += pid->err;

    /* 误差限幅 */

    pid->integral=constrain_float(pid->integral, -pid->integral_max, pid->integral_max);
    pid->p_output = pid->kp * pid->err;
    pid->i_output = pid->ki * pid->integral;
    pid->d_output = pid->kd * (pid->err - pid->err_last);

    pid->err_last = pid->err;

    pid->output = constrain_float(pid->p_output + pid->i_output + pid->d_output,pid->output_min,pid->output_max);

    return pid->output;
}


/*************************************************************************
*  函数名称：PidIncCtrl(pid_param_t * pid, float err)
*  功能说明：pid增量式控制器输出
*  参数说明：
* @param    pid     pid参数
* @param    err   pid输入误差
*  函数返回：PID输出结果   注意输出结果已经包涵了上次结果
*************************************************************************/
float PID_Inc_Ctrl(PID * pid, float err)
{
    pid->p_output = pid->kp * (err - pid->err_last);
    // pid->i_output = pid->ki * err;
    pid->i_output = pid->ki * constrain_float(err, -pid->integral_max, pid->integral_max);
    pid->d_output = pid->kd * ((err - pid->err_last) - pid->last_derivative);

    pid->last_derivative = err - pid->err_last;
    pid->err_last = err;

    pid->output += pid->p_output + pid->i_output + pid->d_output;
    pid->output = constrain_float(pid->output,pid->output_min,pid->output_max);
    return pid->output;
}


float PID_ChangeIntegral_Inc_Ctrl(PID * pid, float err, float change_integral_b, float change_integral_k)
{
    pid->p_output = pid->kp * (err - pid->err_last);
    // pid->i_output = pid->ki * err;
    float ki_c = pid->ki;
    if(err + pid->err_last > 0)
    {
        ki_c = pid->ki - pid->ki / (1 + expf(change_integral_b - fabsf(err) * change_integral_k));
    }
    pid->i_output = ki_c * constrain_float(err, -pid->integral_max, pid->integral_max);
    pid->d_output = pid->kd * ((err - pid->err_last) - pid->last_derivative);

    pid->last_derivative = err - pid->err_last;
    pid->err_last = err;

    pid->output += pid->p_output + pid->i_output + pid->d_output;
    pid->output = constrain_float(pid->output,pid->output_min,pid->output_max);
    return pid->output;
}