84 lines
1.9 KiB
C
84 lines
1.9 KiB
C
/*
|
|
* pid.c
|
|
*
|
|
* Created on: Jan 18, 2022
|
|
* Author: mcfly
|
|
*/
|
|
|
|
#include "pid.h"
|
|
|
|
void PID_Init(PIDController *pid) {
|
|
//Clear controller values
|
|
pid->integrator = 0.0f;
|
|
pid->prevError = 0.0f;
|
|
|
|
pid->differentiator = 0.0f;
|
|
pid->prevMeasurement = 0.0f;
|
|
|
|
pid->out = 0.0f;
|
|
}
|
|
|
|
|
|
float PID_Update(PIDController *pid, float setpoint, float measurement) {
|
|
// PID = Kp + Ki * 1/s + Kd * s/(s*tau+1)
|
|
|
|
// e = error to the setpoint
|
|
// p[n] = Kp * e[n]
|
|
// i[n] = Ki*T/2 * (e[n]+e[n-1]) + i[n-1]
|
|
// d[n] = 2*Kd/(2*tau+T) * (e[n]-e[n-1]) + (2*tau-T)/(2*tau+T) * d[n-1]
|
|
// PID = p[n]+i[n]+d[n]
|
|
|
|
|
|
//Error signal
|
|
float error = setpoint - measurement;
|
|
|
|
//Proportional
|
|
float proportional = pid->Kp * error;
|
|
|
|
//Integral
|
|
pid->integrator += 0.5f * pid->Ki * pid->T * (error + pid->prevError);
|
|
|
|
//Anti-wind-up via dynamic integrator clamping
|
|
float limMinInt, limMaxInt;
|
|
|
|
//Compute integrator limits
|
|
if (pid->limMax > proportional) {
|
|
limMaxInt = pid->limMax - proportional;
|
|
} else {
|
|
limMaxInt = 0.0f;
|
|
}
|
|
if (pid->limMin < proportional) {
|
|
limMinInt = pid->limMin - proportional;
|
|
} else {
|
|
limMinInt = 0.0f;
|
|
}
|
|
|
|
//Clamp integrator
|
|
if (pid->integrator > limMaxInt) {
|
|
pid->integrator = limMaxInt;
|
|
} else if (pid->integrator < limMinInt) {
|
|
pid->integrator = limMinInt;
|
|
}
|
|
|
|
//Derivative (band-limited differentiator)
|
|
pid->differentiator = (2.0f * pid->Kd * (measurement - pid->prevMeasurement) //Note: derivative on measurement
|
|
+ (2.0f * pid->tau - pid->T) * pid->differentiator)
|
|
/ (2.0f * pid->tau + pid->T);
|
|
|
|
//Compute output and apply limits
|
|
pid->out = proportional + pid->integrator + pid->differentiator;
|
|
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;
|
|
|
|
}
|