lhye200/TC264_300W_Power
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化仓库

Browse Source
This commit is contained in:
lhye200
2025-10-15 00:22:56 +08:00
commit a6ddc01c98
650 changed files with 438085 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
code/pid.c Normal file
View File

@@ -0,0 +1,136 @@
// <20>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD>: GB18030
/*
* pid.c
*
* Created on: 2025<32><35>3<EFBFBD><33>25<32><35>
* 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;
}
/*************************************************************************
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>float constrain_float(float amt, float low, float high)
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>
* @param amt <20><> <20><><EFBFBD><EFBFBD>
* @param low <20><> <20><><EFBFBD><EFBFBD>ֵ
* @param high <20><> <20><><EFBFBD><EFBFBD>ֵ
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><D8A3>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
*************************************************************************/
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;
}
/*************************************************************************
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>float PidLocCtrl(pid_param_t * pid, float err)
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>pidλ<64><CEBB>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>
* @param pid pid<69><64><EFBFBD><EFBFBD>
* @param err pid<69><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*************************************************************************/
float PID_Loc_Ctrl(PID * pid, float err)
{
/* <20>ۻ<EFBFBD><DBBB><EFBFBD><EFBFBD><EFBFBD> */
pid->err = err;
pid->integral += pid->err;
/* <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD> */
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;
}
/*************************************************************************
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>PidIncCtrl(pid_param_t * pid, float err)
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>pid<69><64><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>
* @param pid pid<69><64><EFBFBD><EFBFBD>
* @param err pid<69><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ע<><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴν<CFB4><CEBD><EFBFBD>
*************************************************************************/
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;
}