104 lines
2.3 KiB
C
104 lines
2.3 KiB
C
#include "PID.h"
|
|
|
|
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;
|
|
|
|
pid->tau = tau;
|
|
pid->limMin = limMin;
|
|
pid->limMax = limMax;
|
|
pid->limMinInt = limMinInt;
|
|
pid->limMaxInt = limMaxInt;
|
|
pid->T = T;
|
|
|
|
/* Clear controller variables */
|
|
pid->integrator = 0.0f;
|
|
pid->prevError = 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) {
|
|
|
|
/*
|
|
* Error signal
|
|
*/
|
|
float error = setpoint - measurement;
|
|
|
|
|
|
/*
|
|
* Proportional
|
|
*/
|
|
pid->proportional = pid->Kp * error;
|
|
|
|
|
|
/*
|
|
* Integral
|
|
*/
|
|
pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
|
|
|
|
/* Anti-wind-up via integrator clamping */
|
|
if (pid->integrator > pid->limMaxInt) {
|
|
|
|
pid->integrator = pid->limMaxInt;
|
|
|
|
} else if (pid->integrator < pid->limMinInt) {
|
|
|
|
pid->integrator = pid->limMinInt;
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
* 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;
|
|
|
|
}
|
|
|
|
if (pid->out > pid->limMax) {
|
|
|
|
pid->out = pid->limMax;
|
|
|
|
} else if (pid->out < pid->limMin) {
|
|
|
|
pid->out = pid->limMin;
|
|
|
|
}
|
|
|
|
/* Store error and measurement for later use */
|
|
pid->prevError = error;
|
|
pid->prevMeasurement = measurement;
|
|
|
|
/* Return controller output */
|
|
return pid->out;
|
|
}
|