asd/Infantry_new
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加入PID

Browse Source
This commit is contained in:
Wei Zitong
2026-07-15 17:09:24 +08:00
parent 050ae25e9c
commit 8b24a3da19
10 changed files with 139 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
Core/Src/pid.c Normal file
View File

@@ -0,0 +1,26 @@
#include "pid.h"
void pid_init(pid_t *pid, float kp, float ki, float kd, float max_out, float max_i)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->integral = 0.0f;
pid->last_error = 0.0f;
pid->max_out = max_out;
pid->max_integral = max_i;
}
float pid_calc(pid_t *pid, float target, float actual)
{
float error = target - actual;
pid->integral += error;
if (pid->integral > pid->max_integral) pid->integral = pid->max_integral;
if (pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
float output = pid->kp * error + pid->ki * pid->integral
+ pid->kd * (error - pid->last_error);
pid->last_error = error;
if (output > pid->max_out) output = pid->max_out;
if (output < -pid->max_out) output = -pid->max_out;
return output;
}