diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/PID Controller Implementation in Software.pdf b/PID Controller Implementation in Software.pdf new file mode 100644 index 0000000..ca8f0de Binary files /dev/null and b/PID Controller Implementation in Software.pdf differ diff --git a/PID.c b/PID.c index 50b534b..ad70075 100644 --- a/PID.c +++ b/PID.c @@ -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; + } diff --git a/PID.h b/PID.h index 5f7b46b..bf6b885 100644 --- a/PID.h +++ b/PID.h @@ -1,51 +1,43 @@ #ifndef PID_CONTROLLER_H #define PID_CONTROLLER_H -#include - typedef struct { - /* Controller gains */ - float Kp; - float Ki; - float Kd; + /* Controller gains */ + float Kp; + float Ki; + float Kd; - /* Default controller gains */ - float KpDefault; - float KiDefault; - float KdDefault; + /* Derivative low-pass filter time constant */ + float tau; - /* Derivative low-pass filter time constant */ - float tau; + /* Output limits */ + float limMin; + float limMax; + + /* Integrator limits */ + float limMinInt; + float limMaxInt; - /* Output limits */ - float limMin; - float limMax; + /* Sample time (in seconds) */ + float T; - /* Integrator limits */ - float limMinInt; - float limMaxInt; + /* pid coefficient offset for start pid */ + float offset; - /* Sample time (in seconds) */ - float T; + /* Controller "memory" */ + float integrator; + float prevError; /* Required for integrator */ + float proportional; + float differentiator; + float prevMeasurement; /* Required for differentiator */ - bool enable; + /* Controller output */ + float out; - /* Controller "memory" */ - float proportional; - float integrator; - float prevError; /* Required for integrator */ - float differentiator; - float prevMeasurement; /* Required for differentiator */ +} PIDController; - /* Controller output */ - float out; +void PIDController_Init(PIDController *pid); +float PIDController_Update(PIDController *pid, float setpoint, float measurement); -} PidController; - -void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax, - float limMinInt, float limMaxInt, float T); - -float pidControllerUpdate(PidController *pid, float setpoint, float measurement); - -#endif //PID_CONTROLLER_H +#endif diff --git a/PID_Test.txt b/PID_Test.txt new file mode 100644 index 0000000..f796fc7 --- /dev/null +++ b/PID_Test.txt @@ -0,0 +1,65 @@ +#include +#include + +#include "PID.h" + +/* Controller parameters */ +#define PID_KP 2.0f +#define PID_KI 0.5f +#define PID_KD 0.25f + +#define PID_TAU 0.02f + +#define PID_LIM_MIN -10.0f +#define PID_LIM_MAX 10.0f + +#define PID_LIM_MIN_INT -5.0f +#define PID_LIM_MAX_INT 5.0f + +#define SAMPLE_TIME_S 0.01f + +/* Maximum run-time of simulation */ +#define SIMULATION_TIME_MAX 4.0f + +/* Simulated dynamical system (first order) */ +float TestSystem_Update(float inp); + +int main() +{ + /* Initialise PID controller */ + PIDController pid = { PID_KP, PID_KI, PID_KD, + PID_TAU, + PID_LIM_MIN, PID_LIM_MAX, + PID_LIM_MIN_INT, PID_LIM_MAX_INT, + SAMPLE_TIME_S }; + + PIDController_Init(&pid); + + /* Simulate response using test system */ + float setpoint = 1.0f; + + printf("Time (s)\tSystem Output\tControllerOutput\r\n"); + for (float t = 0.0f; t <= SIMULATION_TIME_MAX; t += SAMPLE_TIME_S) { + + /* Get measurement from system */ + float measurement = TestSystem_Update(pid.out); + + /* Compute new control signal */ + PIDController_Update(&pid, setpoint, measurement); + + printf("%f\t%f\t%f\r\n", t, measurement, pid.out); + + } + + return 0; +} + +float TestSystem_Update(float inp) { + + static float output = 0.0f; + static const float alpha = 0.02f; + + output = (SAMPLE_TIME_S * inp + output) / (1.0f + alpha * SAMPLE_TIME_S); + + return output; +} diff --git a/README.md b/README.md new file mode 100644 index 0000000..6b8b41a --- /dev/null +++ b/README.md @@ -0,0 +1,5 @@ +# PID +PID controller implementation written in C. + +Note on 'derivative-on-measurement': Since the 'error signal' effectively going into the differentiator does not depend on the setpoint: e[n] = 0 - measurement, and therefore (e[n] - e[n - 1]) = (0 - measurement) - (0 - prevMeasurement) = -Kd * (measurement - prevMeasurement). (Note the minus sign compared to derivative-on-error!) +I've included the minus sign in the code, so gains will have the effect as normal.