Alekseev/PID
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update pid, work version

Browse Source
This commit is contained in:
Alekseev
2025-05-21 18:24:53 +07:00
parent 484260d6cb
commit ff8192e65e
6 changed files with 134 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
PID.c
View File

@@ -1,55 +1,38 @@
#include "PID.h"
/*test push*/
void PIDController_Init(PIDController *pid) {
void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
float limMinInt, float limMaxInt, float T) {
pid->Kp = Kp;
pid->Ki = Ki;
pid->Kd = Kd;
pid->KpDefault = Kp;
pid->KiDefault = Ki;
pid->KdDefault = Kd;
/* Clear controller variables */
pid->integrator = 0.0f;
pid->prevError = 0.0f;
pid->tau = tau;
pid->limMin = limMin;
pid->limMax = limMax;
pid->limMinInt = limMinInt;
pid->limMaxInt = limMaxInt;
pid->T = T;
pid->differentiator = 0.0f;
pid->prevMeasurement = 0.0f;
/* Clear controller variables */
pid->integrator = 0.0f;
pid->prevError = 0.0f;
pid->out = 0.0f;
pid->differentiator = 0.0f;
pid->prevMeasurement = 0.0f;
pid->out = 0.0f;
pid->enable = false;
}
float pidControllerUpdate(PidController *pid, float setpoint, float measurement) {
float PIDController_Update(PIDController *pid, float setpoint, float measurement) {
/*
* Error signal
*/
/*
* Error signal
*/
float error = setpoint - measurement;
/*
* Proportional
*/
/*
* Proportional
*/
pid->proportional = pid->Kp * error;
/*
* Integral
*/
/*
* Integral
*/
pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
/* Anti-wind-up via integrator clamping */
/* Anti-wind-up via integrator clamping */
if (pid->integrator > pid->limMaxInt) {
pid->integrator = pid->limMaxInt;
@@ -61,30 +44,19 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
}
/*
* Derivative (band-limited differentiator)
*/
pid->differentiator = -(2.0f * pid->Kd * (measurement -
pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
+ (2.0f * pid->tau - pid->T) * pid->differentiator)
/ (2.0f * pid->tau + pid->T);
/*
* Derivative (band-limited differentiator)
*/
pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement) /* Note: derivative on measurement, therefore minus sign in front of equation! */
+ (2.0f * pid->tau - pid->T) * pid->differentiator)
/ (2.0f * pid->tau + pid->T);
/*
* Compute output and apply limits
*/
pid->out = pid->proportional + pid->integrator + pid->differentiator;
if (pid->proportional > pid->limMax) {
pid->proportional = pid->limMax;
} else if (pid->proportional < pid->limMin) {
pid->proportional = pid->limMin;
}
/*
* Compute output and apply limits
*/
pid->out = pid->offset + pid->proportional + pid->integrator + pid->differentiator;
if (pid->out > pid->limMax) {
@@ -96,10 +68,11 @@ float pidControllerUpdate(PidController *pid, float setpoint, float measurement)
}
/* Store error and measurement for later use */
pid->prevError = error;
/* Store error and measurement for later use */
pid->prevError = error;
pid->prevMeasurement = measurement;
/* Return controller output */
/* Return controller output */
return pid->out;
}