137 lines
3.8 KiB
C
137 lines
3.8 KiB
C
// 文件编码: 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;
|
||
}
|
||
|